From d400d6bb7c43d0946a0cd42a61db07ac5efe0161 Mon Sep 17 00:00:00 2001 From: yuzhongchun17 Date: Sun, 11 Aug 2024 17:50:09 -0700 Subject: [PATCH] cleanup and execution fix in joint traj controller based on #408 and change the velocity controller publisher rate to prevent undershoot for j2n6s300 --- .../kinova_joint_trajectory_controller.cpp | 166 ++++++------------ 1 file changed, 56 insertions(+), 110 deletions(-) diff --git a/kinova_driver/src/kinova_joint_trajectory_controller.cpp b/kinova_driver/src/kinova_joint_trajectory_controller.cpp index 91fae91eb..4a06f625f 100644 --- a/kinova_driver/src/kinova_joint_trajectory_controller.cpp +++ b/kinova_driver/src/kinova_joint_trajectory_controller.cpp @@ -8,35 +8,25 @@ JointTrajectoryController::JointTrajectoryController(kinova::KinovaComm &kinova_ kinova_comm_(kinova_comm), nh_(n) { - //ROS_DEBUG_STREAM_ONCE("Get in: " << __PRETTY_FUNCTION__); - ros::NodeHandle pn("~"); std::string robot_type; nh_.param("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); + timer_pub_joint_vel_ = nh_.createTimer(ros::Duration(0.0085), &JointTrajectoryController::pub_joint_vel, this, false, false); terminate_thread_ = false; thread_update_state_ = new boost::thread(boost::bind(&JointTrajectoryController::update_state, this)); @@ -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) + for(int actuator = 0; actuator < number_joint_; actuator++) { - 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) - { - 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