From e785ce0860e1b5a8e65e2df5e4c0d415c450ba9d Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Tue, 30 Aug 2016 11:49:25 +0200 Subject: [PATCH] Added the servoj gain and servoj lookahead time as a parameter at launch time. Resolves #47 --- include/ur_modern_driver/ur_driver.h | 6 +++++- launch/ur10_ros_control.launch | 3 ++- launch/ur3_ros_control.launch | 3 ++- launch/ur5_ros_control.launch | 3 ++- src/ur_driver.cpp | 30 ++++++++++++++++++++++++---- src/ur_ros_wrapper.cpp | 16 ++++++++++++++- 6 files changed, 52 insertions(+), 9 deletions(-) diff --git a/include/ur_modern_driver/ur_driver.h b/include/ur_modern_driver/ur_driver.h index 4781f4f..0f392fa 100644 --- a/include/ur_modern_driver/ur_driver.h +++ b/include/ur_modern_driver/ur_driver.h @@ -50,6 +50,8 @@ private: double servoj_time_; bool executing_traj_; double firmware_version_; + double servoj_lookahead_time_; + double servoj_gain_; public: UrRealtimeCommunication* rt_interface_; UrCommunication* sec_interface_; @@ -58,7 +60,7 @@ public: std::condition_variable& msg_cond, std::string host, 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.); + double max_payload = 1., double servoj_lookahead_time=0.03, double servoj_gain=300.); bool start(); void halt(); @@ -91,6 +93,8 @@ public: void setMinPayload(double m); void setMaxPayload(double m); void setServojTime(double t); + void setServojLookahead(double t); + void setServojGain(double g); }; diff --git a/launch/ur10_ros_control.launch b/launch/ur10_ros_control.launch index 3004def..4059c89 100644 --- a/launch/ur10_ros_control.launch +++ b/launch/ur10_ros_control.launch @@ -19,12 +19,13 @@ - + + diff --git a/launch/ur3_ros_control.launch b/launch/ur3_ros_control.launch index 7150955..4d1f684 100644 --- a/launch/ur3_ros_control.launch +++ b/launch/ur3_ros_control.launch @@ -19,12 +19,13 @@ - + + diff --git a/launch/ur5_ros_control.launch b/launch/ur5_ros_control.launch index d73ae7f..26308a9 100644 --- a/launch/ur5_ros_control.launch +++ b/launch/ur5_ros_control.launch @@ -19,12 +19,13 @@ - + + diff --git a/src/ur_driver.cpp b/src/ur_driver.cpp index db161a9..2208115 100644 --- a/src/ur_driver.cpp +++ b/src/ur_driver.cpp @@ -22,10 +22,10 @@ UrDriver::UrDriver(std::condition_variable& rt_msg_cond, std::condition_variable& msg_cond, std::string host, unsigned int reverse_port, double servoj_time, unsigned int safety_count_max, double max_time_step, double min_payload, - double max_payload) : + double max_payload, double servoj_lookahead_time, double servoj_gain) : REVERSE_PORT_(reverse_port), maximum_time_step_(max_time_step), minimum_payload_( min_payload), maximum_payload_(max_payload), servoj_time_( - servoj_time) { + servoj_time), servoj_lookahead_time_(servoj_lookahead_time), servoj_gain_(servoj_gain) { char buffer[256]; struct sockaddr_in serv_addr; int n, flag; @@ -182,8 +182,8 @@ bool UrDriver::uploadProg() { cmd_str += "\t\t\telif state == SERVO_RUNNING:\n"; if (sec_interface_->robot_state_->getVersion() >= 3.1) - sprintf(buf, "\t\t\t\tservoj(q, t=%.4f, lookahead_time=0.03)\n", - servoj_time_); + sprintf(buf, "\t\t\t\tservoj(q, t=%.4f, lookahead_time=%.4f, gain=%.0f)\n", + servoj_time_, servoj_lookahead_time_, servoj_gain_); else sprintf(buf, "\t\t\t\tservoj(q, t=%.4f)\n", servoj_time_); cmd_str += buf; @@ -358,3 +358,25 @@ void UrDriver::setServojTime(double t) { servoj_time_ = 0.008; } } +void UrDriver::setServojLookahead(double t){ + if (t > 0.03) { + if (t < 0.2) { + servoj_lookahead_time_ = t; + } else { + servoj_lookahead_time_ = 0.2; + } + } else { + servoj_lookahead_time_ = 0.03; + } +} +void UrDriver::setServojGain(double g){ + if (g > 100) { + if (g < 2000) { + servoj_gain_ = g; + } else { + servoj_gain_ = 2000; + } + } else { + servoj_gain_ = 100; + } +} diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index cee1988..5fec120 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -86,7 +86,7 @@ public: as_(nh_, "follow_joint_trajectory", boost::bind(&RosWrapper::goalCB, this, _1), boost::bind(&RosWrapper::cancelCB, this, _1), false), robot_( - rt_msg_cond_, msg_cond_, host, reverse_port), io_flag_delay_(0.05), joint_offsets_( + rt_msg_cond_, msg_cond_, host, reverse_port, 0.03, 300), io_flag_delay_(0.05), joint_offsets_( 6, 0.0) { std::string joint_prefix = ""; @@ -158,6 +158,20 @@ public: } robot_.setServojTime(servoj_time); + double servoj_lookahead_time = 0.03; + if (ros::param::get("~servoj_lookahead_time", servoj_lookahead_time)) { + sprintf(buf, "Servoj_lookahead_time set to: %f [sec]", servoj_lookahead_time); + print_debug(buf); + } + robot_.setServojLookahead(servoj_lookahead_time); + + double servoj_gain = 300.; + if (ros::param::get("~servoj_gain", servoj_gain)) { + sprintf(buf, "Servoj_gain set to: %f [sec]", servoj_gain); + print_debug(buf); + } + robot_.setServojGain(servoj_gain); + //Base and tool frames base_frame_ = joint_prefix + "base_link"; tool_frame_ = joint_prefix + "tool0_controller";