diff --git a/include/ur_modern_driver/ur_driver.h b/include/ur_modern_driver/ur_driver.h index fc98214..de74a82 100644 --- a/include/ur_modern_driver/ur_driver.h +++ b/include/ur_modern_driver/ur_driver.h @@ -39,13 +39,14 @@ private: const unsigned int REVERSE_PORT_; int incoming_sockfd_; int new_sockfd_; + double servoj_time_; public: UrRealtimeCommunication* rt_interface_; UrCommunication* sec_interface_; UrDriver(std::condition_variable& rt_msg_cond, std::condition_variable& msg_cond, std::string host, - unsigned int reverse_port = 50007, unsigned int safety_count_max = + unsigned int reverse_port = 50007, double servoj_time = 0.016, unsigned int safety_count_max = 12, double max_time_step = 0.08, double min_payload = 0., double max_payload = 1.); bool start(); @@ -53,14 +54,14 @@ public: void setSpeed(double q0, double q1, double q2, double q3, double q4, double q5, double acc = 100.); - void addTraj( + /* void addTraj( std::vector inp_timestamps, //DEPRECATED std::vector > positions, - std::vector > velocities); + std::vector > velocities); */ void doTraj(std::vector inp_timestamps, std::vector > inp_positions, std::vector > inp_velocities); - void servoj(std::vector positions, double time, int keepalive); + void servoj(std::vector positions, int keepalive = 1, double time = 0); void stopTraj(); @@ -82,6 +83,7 @@ public: void setMinPayload(double m); void setMaxPayload(double m); + void setServojTime(double t); }; diff --git a/src/do_output.cpp b/src/do_output.cpp index a14b174..a4b8b68 100644 --- a/src/do_output.cpp +++ b/src/do_output.cpp @@ -13,35 +13,35 @@ void print_debug(std::string inp) { #ifdef ROS_BUILD - ROS_DEBUG(inp.c_str()); + ROS_DEBUG("%s", inp.c_str()); #else printf("DEBUG: %s\n", inp.c_str()); #endif } void print_info(std::string inp) { #ifdef ROS_BUILD - ROS_INFO(inp.c_str()); + ROS_INFO("%s", inp.c_str()); #else printf("INFO: %s\n", inp.c_str()); #endif } void print_warning(std::string inp) { #ifdef ROS_BUILD - ROS_WARN(inp.c_str()); + ROS_WARN("%s", inp.c_str()); #else printf("WARNING: %s\n", inp.c_str()); #endif } void print_error(std::string inp) { #ifdef ROS_BUILD - ROS_ERROR(inp.c_str()); + ROS_ERROR("%s", inp.c_str()); #else printf("ERROR: %s\n", inp.c_str()); #endif } void print_fatal(std::string inp) { #ifdef ROS_BUILD - ROS_FATAL(inp.c_str()); + ROS_FATAL("%s", inp.c_str()); ros::shutdown(); #else printf("FATAL: %s\n", inp.c_str()); diff --git a/src/ur_driver.cpp b/src/ur_driver.cpp index 9a60858..c141ec3 100644 --- a/src/ur_driver.cpp +++ b/src/ur_driver.cpp @@ -13,10 +13,10 @@ UrDriver::UrDriver(std::condition_variable& rt_msg_cond, std::condition_variable& msg_cond, std::string host, - unsigned int reverse_port, unsigned int safety_count_max, + unsigned int reverse_port, double servoj_time, unsigned int safety_count_max, double max_time_step, double min_payload, double max_payload) : REVERSE_PORT_(reverse_port), maximum_time_step_(max_time_step), minimum_payload_( - min_payload), maximum_payload_(max_payload) { + min_payload), maximum_payload_(max_payload), servoj_time_(servoj_time) { char buffer[256]; struct sockaddr_in serv_addr; int n, flag; @@ -63,11 +63,11 @@ std::vector UrDriver::interp_cubic(double t, double T, } return positions; } - + /* void UrDriver::addTraj(std::vector inp_timestamps, std::vector > inp_positions, std::vector > inp_velocities) { - /* DEPRECATED */ + // DEPRECATED printf("!! addTraj is deprecated !!\n"); std::vector timestamps; std::vector > positions; @@ -82,11 +82,11 @@ void UrDriver::addTraj(std::vector inp_timestamps, timestamps.push_back(inp_timestamps[i - 1] + step_size * j); } } - //make sure we come to a smooth stop - /*while (timestamps.back() < inp_timestamps.back()) { - timestamps.push_back(timestamps.back() + 0.008); - } - timestamps.pop_back();*/ + // //make sure we come to a smooth stop + // while (timestamps.back() < inp_timestamps.back()) { + // timestamps.push_back(timestamps.back() + 0.008); + // } + // timestamps.pop_back(); unsigned int j = 0; for (unsigned int i = 0; i < timestamps.size(); i++) { @@ -102,7 +102,7 @@ void UrDriver::addTraj(std::vector inp_timestamps, timestamps.push_back(inp_timestamps[inp_timestamps.size() - 1]); positions.push_back(inp_positions[inp_positions.size() - 1]); - /* This is actually faster than using a stringstream :-o */ + /// This is actually faster than using a stringstream :-o for (unsigned int i = 1; i < timestamps.size(); i++) { char buf[128]; sprintf(buf, @@ -117,7 +117,7 @@ void UrDriver::addTraj(std::vector inp_timestamps, rt_interface_->addCommandToQueue(command_string); } - +*/ void UrDriver::doTraj(std::vector inp_timestamps, std::vector > inp_positions, std::vector > inp_velocities) { @@ -144,21 +144,24 @@ void UrDriver::doTraj(std::vector inp_timestamps, inp_timestamps[j] - inp_timestamps[j - 1], inp_positions[j - 1], inp_positions[j], inp_velocities[j - 1], inp_velocities[j]); - UrDriver::servoj(positions, 0.008, 1); + UrDriver::servoj(positions); // oversample with 4 * sample_time - std::this_thread::sleep_for(std::chrono::milliseconds(2)); + std::this_thread::sleep_for(std::chrono::milliseconds((int) ((servoj_time_*1000)/4.))); t = std::chrono::high_resolution_clock::now(); } //Signal robot to stop driverProg() UrDriver::closeServo(positions); } -void UrDriver::servoj(std::vector positions, double time, - int keepalive) { +void UrDriver::servoj(std::vector positions, + int keepalive, double time) { unsigned int bytes_written; int tmp; unsigned char buf[32]; + if (time < 0.016) { + time = servoj_time_; + } for (int i = 0; i < 6; i++) { tmp = htonl((int) (positions[i] * MULT_JOINTSTATE_)); buf[i * 4] = tmp & 0xff; @@ -272,9 +275,9 @@ void UrDriver::openServo() { } void UrDriver::closeServo(std::vector positions) { if (positions.size() != 6) - UrDriver::servoj(rt_interface_->robot_state_->getQActual(), 0.008, 0); + UrDriver::servoj(rt_interface_->robot_state_->getQActual(), 0); else - UrDriver::servoj(positions, 0.008, 0); + UrDriver::servoj(positions, 0); close(new_sockfd_); } @@ -352,8 +355,20 @@ bool UrDriver::setPayload(double m) { } void UrDriver::setMinPayload(double m) { - minimum_payload_ = m; + if (m > 0) { + minimum_payload_ = m; + } else { + minimum_payload_ = 0; + } + } void UrDriver::setMaxPayload(double m) { maximum_payload_ = m; } +void UrDriver::setServojTime(double t) { + if (t > 0.016) { + servoj_time_ = t; + } else { + servoj_time_ = 0.016; + } +} diff --git a/src/ur_hardware_interface.cpp b/src/ur_hardware_interface.cpp index a96e87d..60dfadc 100644 --- a/src/ur_hardware_interface.cpp +++ b/src/ur_hardware_interface.cpp @@ -141,7 +141,7 @@ void UrHardwareInterface::write() { joint_velocity_command_[2], joint_velocity_command_[3], joint_velocity_command_[4], joint_velocity_command_[5], 100); } else if (position_interface_running_) { - robot_->servoj(joint_position_command_, 0.008, 1); + robot_->servoj(joint_position_command_); } } diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index d2066fc..2035161 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -75,7 +75,7 @@ public: char buf[256]; if (ros::param::get("~prefix", joint_prefix)) { - sprintf(buf,"Setting prefix to %s", joint_prefix.c_str()); + sprintf(buf, "Setting prefix to %s", joint_prefix.c_str()); print_info(buf); } joint_names.push_back(joint_prefix + "shoulder_pan_joint"); @@ -122,6 +122,13 @@ public: min_payload, max_payload); print_debug(buf); + double servoj_time = 0.016; + if (ros::param::get("~servoj_time", servoj_time)) { + sprintf(buf, "Servoj_time set to: %f [sec]", servoj_time); + print_debug(buf); + } + robot_.setServojTime(servoj_time); + if (robot_.start()) { if (use_ros_control_) { ros_control_thread_ = new std::thread( @@ -140,7 +147,8 @@ public: //subscribe to the data topic of interest rt_publish_thread_ = new std::thread( boost::bind(&RosWrapper::publishRTMsg, this)); - print_debug("The action server for this driver has been started"); + print_debug( + "The action server for this driver has been started"); } mb_publish_thread_ = new std::thread( boost::bind(&RosWrapper::publishMbMsg, this));