diff --git a/kinova_driver/include/kinova/KinovaTypes.h b/kinova_driver/include/kinova/KinovaTypes.h index c09212c61..3e83cdf03 100644 --- a/kinova_driver/include/kinova/KinovaTypes.h +++ b/kinova_driver/include/kinova/KinovaTypes.h @@ -468,6 +468,31 @@ struct AngularInfo Actuator6 = 0.0f; Actuator7 = 0.0f; } + + float& operator [](const int idx){ + switch (idx) + { + case 0: return Actuator1; + case 1: return Actuator2; + case 2: return Actuator3; + case 3: return Actuator4; + case 4: return Actuator5; + case 5: return Actuator6; + case 6: return Actuator7; + } + } + float operator [](const int idx) const{ + switch (idx) + { + case 0: return Actuator1; + case 1: return Actuator2; + case 2: return Actuator3; + case 3: return Actuator4; + case 4: return Actuator5; + case 5: return Actuator6; + case 6: return Actuator7; + } + } }; /** @brief This data structure holds values in an cartesian control context. diff --git a/kinova_driver/include/kinova_driver/kinova_arm.h b/kinova_driver/include/kinova_driver/kinova_arm.h index c0e194055..f8bb703b2 100644 --- a/kinova_driver/include/kinova_driver/kinova_arm.h +++ b/kinova_driver/include/kinova_driver/kinova_arm.h @@ -18,6 +18,8 @@ #include #include #include +#include +#include #include #include @@ -78,6 +80,8 @@ class KinovaArm // Service callbacks ----------------------------------------------------------- bool stopServiceCallback(kinova_msgs::Stop::Request &req, kinova_msgs::Stop::Response &res); bool startServiceCallback(kinova_msgs::Start::Request &req, kinova_msgs::Start::Response &res); + bool cartesian_controlServiceCallback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res); + bool angular_controlServiceCallback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res); bool homeArmServiceCallback(kinova_msgs::HomeArm::Request &req, kinova_msgs::HomeArm::Response &res); bool ActivateNullSpaceModeCallback(kinova_msgs::SetNullSpaceModeState::Request &req, kinova_msgs::SetNullSpaceModeState::Response &res); @@ -116,6 +120,7 @@ class KinovaArm void publishToolPosition(void); void publishToolWrench(void); void publishFingerPosition(void); + void publishJoystickCommand(void); tf::TransformListener tf_listener_; ros::NodeHandle node_handle_; @@ -135,12 +140,15 @@ class KinovaArm ros::Publisher tool_wrench_publisher_; ros::Publisher finger_position_publisher_; ros::Publisher joint_state_publisher_; + ros::Publisher joystick_command_publisher_; ros::Publisher joint_command_publisher_; ros::Publisher cartesian_command_publisher_; ros::ServiceServer stop_service_; ros::ServiceServer start_service_; + ros::ServiceServer cartesian_control_service_; + ros::ServiceServer angular_control_service_; ros::ServiceServer homing_service_; ros::ServiceServer start_null_space_service_; ros::ServiceServer add_trajectory_; diff --git a/kinova_driver/include/kinova_driver/kinova_comm.h b/kinova_driver/include/kinova_driver/kinova_comm.h index 8bedde24f..f95f2ffd9 100644 --- a/kinova_driver/include/kinova_driver/kinova_comm.h +++ b/kinova_driver/include/kinova_driver/kinova_comm.h @@ -88,6 +88,7 @@ class KinovaComm void initFingers(void); void setEndEffectorOffset(unsigned int status, float x, float y, float z); void getEndEffectorOffset(unsigned int &status, float &x, float &y, float &z); + void getJoystickCommand(JoystickCommand &joystick_command); // %EndTag(general functions) diff --git a/kinova_driver/include/kinova_driver/kinova_joint_trajectory_controller.h b/kinova_driver/include/kinova_driver/kinova_joint_trajectory_controller.h index 0953c4132..207b65e31 100644 --- a/kinova_driver/include/kinova_driver/kinova_joint_trajectory_controller.h +++ b/kinova_driver/include/kinova_driver/kinova_joint_trajectory_controller.h @@ -1,7 +1,6 @@ #ifndef KINOVA_JOINT_TRAJECTORY_CONTROLLER_H #define KINOVA_JOINT_TRAJECTORY_CONTROLLER_H - #include #include #include @@ -26,46 +25,33 @@ class JointTrajectoryController private: ros::NodeHandle nh_; - ros::Subscriber sub_command_; - ros::Publisher pub_joint_feedback_; - ros::Publisher pub_joint_velocity_; + ros::Subscriber sub_command_; // command subscriber listening to 'trajectory_controller/command' + ros::Publisher pub_joint_feedback_; // feedback publisher sending to 'trajectory_controller/state' + ros::Publisher pub_joint_velocity_; // velocity publisher sending to 'in/joint_velocity' - ros::Time previous_pub_; - ros::Time time_pub_joint_vel_; + ros::Time time_pub_joint_vel_; // time of timer thread start to publish joint velocity command - ros::Timer timer_pub_joint_vel_; + ros::Timer timer_pub_joint_vel_; // timer to publish joint velocities boost::mutex terminate_thread_mutex_; boost::thread* thread_update_state_; bool terminate_thread_; - std_msgs::Duration time_from_start_; - sensor_msgs::JointState current_joint_state_; - - KinovaComm kinova_comm_; - TrajectoryPoint kinova_traj_point_; + KinovaComm kinova_comm_; // Communication pipeline for the arm -// trajectory_msgs::JointTrajectory joint_traj_; -// trajectory_msgs::JointTrajectoryPoint joint_traj_point_; - std::string traj_frame_id_; - std::vector traj_command_points_; - control_msgs::FollowJointTrajectoryFeedback traj_feedback_msg_; + std::string traj_frame_id_; // origin of trajectory transforms + std::vector traj_command_points_; // trajectory points from most recent trajectory + control_msgs::FollowJointTrajectoryFeedback traj_feedback_msg_; // trajectory feedback message // stores the command to send to robot, in Kinova type (KinovaAngles) - std::vector kinova_angle_command_; - - uint number_joint_; - int traj_command_points_index_; - std::vector joint_names_; - std::string prefix_; - - struct Segment - { - double start_time; - double duration; - std::vector positions; - std::vector velocities; - }; + std::vector kinova_angle_command_; // intermediate command storage + uint number_joint_; // number of joints of the robot + int traj_command_points_index_; // current index in traj_command_points_, defined by time + std::vector joint_names_; // names of the joints + std::string prefix_; // robot name prefix + const static int num_possible_joints = 7; // number of possible joints supported by the system + float current_velocity_command[num_possible_joints]; // storage array to keep calculated velocity commands + double remaining_motion_time[num_possible_joints]; // time of motion remaining for each joint during the last command // call back function when receive a trajectory command void commandCB(const trajectory_msgs::JointTrajectoryConstPtr &traj_msg); @@ -73,17 +59,11 @@ class JointTrajectoryController // reflash the robot state and publish the joint state: either by timer or thread void update_state(); // by thread + // regularly publish the velocity commands of the trajectory void pub_joint_vel(const ros::TimerEvent&); // by timer - int test; }; - - - - - } - #endif // KINOVA_JOINT_TRAJECTORY_CONTROLLER_H diff --git a/kinova_driver/src/joint_trajectory_action/joint_trajectory_action_server.cpp b/kinova_driver/src/joint_trajectory_action/joint_trajectory_action_server.cpp index 1f082626b..26f22612d 100644 --- a/kinova_driver/src/joint_trajectory_action/joint_trajectory_action_server.cpp +++ b/kinova_driver/src/joint_trajectory_action/joint_trajectory_action_server.cpp @@ -236,10 +236,12 @@ void JointTrajectoryActionController::controllerStateCB(const control_msgs::Foll { // Checks that we have ended inside the goal constraints bool inside_goal_constraints = true; - for (size_t i = 0; i < msg->joint_names.size() && inside_goal_constraints; ++i) + bool inside_velocity_constraints = true; + for (size_t i = 0; i < msg->joint_names.size() && inside_goal_constraints && inside_velocity_constraints; ++i) { // computing error from goal pose double abs_error = fabs(msg->actual.positions[i] - current_traj_.points[last].positions[i]); + abs_error = abs_error - 2*M_PI*std::floor((abs_error+M_PI)/(2*M_PI)); // fix rotation to [-PI,PI) double goal_constraint = goal_constraints_[msg->joint_names[i]]; if (goal_constraint >= 0 && abs_error > goal_constraint) inside_goal_constraints = false; @@ -247,10 +249,10 @@ void JointTrajectoryActionController::controllerStateCB(const control_msgs::Foll if ( !(msg->desired.velocities.empty()) && (fabs(msg->desired.velocities[i]) < 1e-6) ) { if (fabs(msg->actual.velocities[i]) > stopped_velocity_tolerance_) - inside_goal_constraints = false; + inside_velocity_constraints = false; } } - if (inside_goal_constraints) + if (inside_goal_constraints && inside_velocity_constraints) { active_goal_.setSucceeded(); has_active_goal_ = false; @@ -262,7 +264,17 @@ void JointTrajectoryActionController::controllerStateCB(const control_msgs::Foll } else { - ROS_WARN("Aborting because we wound up outside the goal constraints"); + ROS_WARN("Aborting because we wound up outside the %s constraints", inside_velocity_constraints?"goal":"velocity"); + for(int j = 0; j<7; j++){ + double abs_error; + if(!inside_goal_constraints){ + abs_error = fabs(msg->actual.positions[j] - current_traj_.points[last].positions[j]); + abs_error = abs_error - 2*M_PI*std::floor((abs_error+M_PI)/(2*M_PI)); // fix rotation to [-PI,PI) + } else { + abs_error = fabs(msg->actual.velocities[j] - stopped_velocity_tolerance_); + } + ROS_WARN_STREAM("Error " << j << ": " << abs_error); + } active_goal_.setAborted(); has_active_goal_ = false; } diff --git a/kinova_driver/src/kinova_arm.cpp b/kinova_driver/src/kinova_arm.cpp index 30fc89c34..bc9d85ab3 100644 --- a/kinova_driver/src/kinova_arm.cpp +++ b/kinova_driver/src/kinova_arm.cpp @@ -165,6 +165,8 @@ KinovaArm::KinovaArm(KinovaComm &arm, const ros::NodeHandle &nodeHandle, const s /* Set up Services */ stop_service_ = node_handle_.advertiseService("in/stop", &KinovaArm::stopServiceCallback, this); start_service_ = node_handle_.advertiseService("in/start", &KinovaArm::startServiceCallback, this); + cartesian_control_service_ = node_handle_.advertiseService("in/cartesian_control", &KinovaArm::cartesian_controlServiceCallback, this); + angular_control_service_ = node_handle_.advertiseService("in/angular_control", &KinovaArm::angular_controlServiceCallback, this); homing_service_ = node_handle_.advertiseService("in/home_arm", &KinovaArm::homeArmServiceCallback, this); add_trajectory_ = node_handle_.advertiseService("in/add_pose_to_Cartesian_trajectory", &KinovaArm::addCartesianPoseToTrajectory, this); @@ -202,6 +204,8 @@ KinovaArm::KinovaArm(KinovaComm &arm, const ros::NodeHandle &nodeHandle, const s ("out/tool_wrench", 2); finger_position_publisher_ = node_handle_.advertise ("out/finger_position", 2); + joystick_command_publisher_ = node_handle_.advertise + ("out/joystick_command", 2); // Publish last command for relative motion (read current position cause arm drop) joint_command_publisher_ = node_handle_.advertise("out/joint_command", 2); @@ -370,6 +374,29 @@ bool KinovaArm::stopServiceCallback(kinova_msgs::Stop::Request &req, kinova_msgs return true; } +/* + * Handler for service to change to cartesian control mode. + */ +bool KinovaArm::cartesian_controlServiceCallback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) +{ + kinova_comm_.setCartesianControl(); + res.message = "Cartesian Control activated"; + res.success = true; + ROS_DEBUG("Cartesian Control requested"); + return true; +} + +/* + * Handler for service to change to angular control mode. + */ +bool KinovaArm::angular_controlServiceCallback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) +{ + kinova_comm_.setAngularControl(); + res.message = "Angular Control activated"; + res.success = true; + ROS_DEBUG("Angular Control requested"); + return true; +} /*! * \brief Handler for "start" service. @@ -756,12 +783,41 @@ void KinovaArm::publishFingerPosition(void) finger_position_publisher_.publish(fingers.constructFingersMsg()); } +/** + * \brief Publishes the current command of the joystick + */ +void KinovaArm::publishJoystickCommand(void) +{ + JoystickCommand command; + kinova_comm_.getJoystickCommand(command); + + sensor_msgs::Joy msg; + msg.header.stamp = ros::Time::now(); + msg.header.frame_id = "kinova_joystick"; + msg.axes = {command.InclineLeftRight, + command.InclineForwardBackward, + command.Rotate, + command.MoveLeftRight, + command.MoveForwardBackward, + command.PushPull + }; + // The ButtonValue array seems to be ill-formed: + // Tests show it consists of 1-byte (char) sized values and is actually offset by 4 bytes + const int data_offset_bytes = 4; + const char* actual_button_array = (char*) command.ButtonValue + data_offset_bytes; + for(int i=0; i("robot_name",prefix_,"j2n6s300"); nh_.param("robot_type",robot_type,"j2n6s300"); number_joint_ =robot_type[3] - '0'; - // Display debug information in teminal - if( ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug) ) { - ros::console::notifyLoggerLevelsChanged(); - } - sub_command_ = nh_.subscribe("trajectory_controller/command", 1, &JointTrajectoryController::commandCB, this); pub_joint_feedback_ = nh_.advertise("trajectory_controller/state", 1); pub_joint_velocity_ = pn.advertise("in/joint_velocity", 2); - traj_frame_id_ = "root"; + traj_frame_id_ = "root"; joint_names_.resize(number_joint_); - //std::cout << "joint names in feedback of trajectory state are: " << std::endl; for (uint i = 0; i(i+1); - std::cout << joint_names_[i] << " "; } - std::cout << std::endl; timer_pub_joint_vel_ = nh_.createTimer(ros::Duration(0.01), &JointTrajectoryController::pub_joint_vel, this, false, false); terminate_thread_ = false; @@ -52,13 +42,11 @@ JointTrajectoryController::JointTrajectoryController(kinova::KinovaComm &kinova_ // counter in the timer to publish joint velocity command: pub_joint_vel() traj_command_points_index_ = 0; - - //ROS_DEBUG_STREAM_ONCE("Get out: " << __PRETTY_FUNCTION__); + for(int i=0; ijoin(); delete thread_update_state_; - - //ROS_DEBUG_STREAM_ONCE("Get out: " << __PRETTY_FUNCTION__); } void JointTrajectoryController::commandCB(const trajectory_msgs::JointTrajectoryConstPtr &traj_msg) { - //ROS_DEBUG_STREAM_ONCE("Get in: " << __PRETTY_FUNCTION__); - bool command_abort = false; -// // if receive new command, clear all trajectory and stop api -// kinova_comm_.stopAPI(); -// if(!kinova_comm_.isStopped()) -// { -// ros::Duration(0.01).sleep(); -// } -// kinova_comm_.eraseAllTrajectories(); - -// kinova_comm_.startAPI(); -// if(kinova_comm_.isStopped()) -// { -// ros::Duration(0.01).sleep(); -// } - traj_command_points_ = traj_msg->points; - ROS_INFO_STREAM("Trajectory controller Receive trajectory with points number: " << traj_command_points_.size()); - + ROS_INFO_STREAM_NAMED("Trajectory controller", "Receive trajectory with points number: " << traj_command_points_.size()); + if(traj_command_points_.empty()) return; // abort if // Map the index in joint_names and the msg std::vector lookup(number_joint_, -1); @@ -139,9 +109,15 @@ void JointTrajectoryController::commandCB(const trajectory_msgs::JointTrajectory command_abort = true; break; } - + // velocity should not be empty + if (traj_command_points_[j].velocities.empty()) + { + ROS_ERROR_STREAM("Velocities in trajectory command cannot be empty at point: " << j); + command_abort = true; + break; + } // if velocity provided, size match - if (!traj_command_points_[j].velocities.empty() && traj_command_points_[j].velocities.size() != number_joint_) + if (traj_command_points_[j].velocities.size() != number_joint_) { ROS_ERROR_STREAM("Velocities at point " << j << " has size " << traj_command_points_[j].velocities.size() << " in trajectory command, which does not match joint number! "); command_abort = true; @@ -153,22 +129,13 @@ void JointTrajectoryController::commandCB(const trajectory_msgs::JointTrajectory return; // store angle velocity command sent to robot -// std::vector kinova_angle_command; kinova_angle_command_.resize(traj_command_points_.size()); for (size_t i = 0; i=6) + for(int actuator = 0; actuator < number_joint_; actuator++) { - kinova_angle_command_[i].Actuator5 = traj_command_points_[i].velocities[4] *180/M_PI; - kinova_angle_command_[i].Actuator6 = traj_command_points_[i].velocities[5] *180/M_PI; - if (number_joint_==7) - kinova_angle_command_[i].Actuator7 = traj_command_points_[i].velocities[6] *180/M_PI; + kinova_angle_command_[i][actuator] = traj_command_points_[i].velocities[actuator] *180/M_PI; } } @@ -176,52 +143,65 @@ void JointTrajectoryController::commandCB(const trajectory_msgs::JointTrajectory double trajectory_duration = traj_command_points_[0].time_from_start.toSec(); durations[0] = trajectory_duration; -// ROS_DEBUG_STREAM("durationsn 0 is: " << durations[0]); for (int i = 1; i remaining_motion_time[i]) + current_velocity_command[i] = 0; + } + } + + joint_velocity_msg.joint1 = current_velocity_command[0]; + joint_velocity_msg.joint2 = current_velocity_command[1]; + joint_velocity_msg.joint3 = current_velocity_command[2]; + joint_velocity_msg.joint4 = current_velocity_command[3]; + joint_velocity_msg.joint5 = current_velocity_command[4]; + joint_velocity_msg.joint6 = current_velocity_command[5]; + joint_velocity_msg.joint7 = current_velocity_command[6]; pub_joint_velocity_.publish(joint_velocity_msg); - if( (ros::Time::now() - time_pub_joint_vel_) >= traj_command_points_[traj_command_points_index_].time_from_start) + if( current_time_from_start >= traj_command_points_[traj_command_points_index_].time_from_start) { ROS_INFO_STREAM("Moved to point " << traj_command_points_index_++); + for(int i=0; i=6) - { - traj_feedback_msg_.desired.positions[4] = current_joint_command.Actuators.Actuator5 *M_PI/180; - traj_feedback_msg_.desired.positions[5] = current_joint_command.Actuators.Actuator6 *M_PI/180; - if (number_joint_==7) - traj_feedback_msg_.desired.positions[6] = current_joint_command.Actuators.Actuator7 *M_PI/180; - } - - traj_feedback_msg_.actual.positions[0] = current_joint_angles.Actuator1 *M_PI/180; - traj_feedback_msg_.actual.positions[1] = current_joint_angles.Actuator2 *M_PI/180; - traj_feedback_msg_.actual.positions[2] = current_joint_angles.Actuator3 *M_PI/180; - traj_feedback_msg_.actual.positions[3] = current_joint_angles.Actuator4 *M_PI/180; - if (number_joint_>=6) - { - traj_feedback_msg_.actual.positions[4] = current_joint_angles.Actuator5 *M_PI/180; - traj_feedback_msg_.actual.positions[5] = current_joint_angles.Actuator6 *M_PI/180; - if (number_joint_==7) - traj_feedback_msg_.actual.positions[6] = current_joint_angles.Actuator7 *M_PI/180; - } - - traj_feedback_msg_.actual.velocities[0] = current_joint_velocity.Actuator1 *M_PI/180; - traj_feedback_msg_.actual.velocities[1] = current_joint_velocity.Actuator2 *M_PI/180; - traj_feedback_msg_.actual.velocities[2] = current_joint_velocity.Actuator3 *M_PI/180; - traj_feedback_msg_.actual.velocities[3] = current_joint_velocity.Actuator4 *M_PI/180; - if (number_joint_>=6) + for(int actuator = 0; actuator < number_joint_; actuator++) { - traj_feedback_msg_.actual.velocities[4] = current_joint_velocity.Actuator5 *M_PI/180; - traj_feedback_msg_.actual.velocities[5] = current_joint_velocity.Actuator6 *M_PI/180; - if (number_joint_==7) - traj_feedback_msg_.actual.velocities[6] = current_joint_velocity.Actuator7 *M_PI/180; + traj_feedback_msg_.desired.positions[actuator] = current_joint_command.Actuators[actuator] *M_PI/180; + traj_feedback_msg_.actual.positions[actuator] = current_joint_angles[actuator] *M_PI/180; + traj_feedback_msg_.actual.velocities[actuator] = current_joint_velocity[actuator] *M_PI/180; } for (size_t j = 0; j