From c6dcb51ed1f85e7fc6b73a5aaad0911bc106fe71 Mon Sep 17 00:00:00 2001 From: Dennis Hartmann Date: Mon, 4 Jun 2018 17:22:15 +0200 Subject: [PATCH 1/3] rsi_simulator now runs at fixed 250Hz and increases IPOC by 4 --- kuka_rsi_simulator/scripts/kuka_rsi_simulator | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/kuka_rsi_simulator/scripts/kuka_rsi_simulator b/kuka_rsi_simulator/scripts/kuka_rsi_simulator index 15131e167..638341087 100755 --- a/kuka_rsi_simulator/scripts/kuka_rsi_simulator +++ b/kuka_rsi_simulator/scripts/kuka_rsi_simulator @@ -38,7 +38,7 @@ node_name = 'kuka_rsi_simulation' rsi_act_pub = rospy.Publisher(node_name + '/rsi/state', String, queue_size=1) rsi_cmd_pub = rospy.Publisher(node_name + '/rsi/command', String, queue_size=1) -cycle_time = 0.004 +cycle_time = 4 act_joint_pos = np.array([0, -90, 90, 0, 90, 0]).astype(np.float64) cmd_joint_pos = act_joint_pos.copy() des_joint_correction_absolute = np.zeros(6) @@ -58,6 +58,7 @@ if __name__ == '__main__': sen_type = args.sen rospy.init_node(node_name) + rate = rospy.Rate(1000.0/cycle_time) rospy.loginfo('{}: Started'.format(node_name)) try: @@ -83,8 +84,8 @@ if __name__ == '__main__': rsi_cmd_pub.publish(recv_msg) des_joint_correction_absolute, ipoc_recv = parse_rsi_xml_sen(recv_msg) act_joint_pos = cmd_joint_pos + des_joint_correction_absolute - ipoc += 1 - time.sleep(cycle_time / 2) + ipoc += cycle_time + rate.sleep() except socket.timeout, msg: rospy.logwarn('{}: Socket timed out'.format(node_name)) timeout_count += 1 From 6271a9932ea3e462658528a2e2844cc711957f7e Mon Sep 17 00:00:00 2001 From: Dennis Hartmann Date: Tue, 5 Jun 2018 11:14:59 +0200 Subject: [PATCH 2/3] update controller manager with period taken from ipoc --- .../kuka_rsi_hw_interface/kuka_hardware_interface.h | 4 +--- kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp | 5 +++++ .../src/kuka_hardware_interface_node.cpp | 9 +-------- 3 files changed, 7 insertions(+), 11 deletions(-) diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.h b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.h index 9e1f919b3..2e520a408 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.h +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.h @@ -56,8 +56,6 @@ #include #include -// Timers -#include // UDP server #include @@ -126,7 +124,7 @@ class KukaHardwareInterface : public hardware_interface::RobotHW void configure(); bool read(const ros::Time time, const ros::Duration period); bool write(const ros::Time time, const ros::Duration period); - + ros::Duration getPeriod() const { return control_period_; } }; } // namespace kuka_rsi_hw_interface diff --git a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp index f03f13bb6..216f586da 100644 --- a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp +++ b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp @@ -104,6 +104,11 @@ bool KukaHardwareInterface::read(const ros::Time time, const ros::Duration perio } rsi_state_ = RSIState(in_buffer_); + if(rsi_state_.ipoc > ipoc_) + { + control_period_.fromSec((rsi_state_.ipoc - ipoc_) / 1000.0); + } + for (std::size_t i = 0; i < n_dof_; ++i) { joint_position_[i] = DEG2RAD * rsi_state_.positions[i]; diff --git a/kuka_rsi_hw_interface/src/kuka_hardware_interface_node.cpp b/kuka_rsi_hw_interface/src/kuka_hardware_interface_node.cpp index ba587e348..ccd9c83d5 100644 --- a/kuka_rsi_hw_interface/src/kuka_hardware_interface_node.cpp +++ b/kuka_rsi_hw_interface/src/kuka_hardware_interface_node.cpp @@ -56,8 +56,6 @@ int main(int argc, char** argv) // Set up timers ros::Time timestamp; ros::Duration period; - auto stopwatch_last = std::chrono::steady_clock::now(); - auto stopwatch_now = stopwatch_last; controller_manager::ControllerManager controller_manager(&kuka_rsi_hw_interface, nh); @@ -65,9 +63,6 @@ int main(int argc, char** argv) // Get current time and elapsed time since last read timestamp = ros::Time::now(); - stopwatch_now = std::chrono::steady_clock::now(); - period.fromSec(std::chrono::duration_cast>(stopwatch_now - stopwatch_last).count()); - stopwatch_last = stopwatch_now; // Run as fast as possible while (ros::ok()) @@ -82,9 +77,7 @@ int main(int argc, char** argv) // Get current time and elapsed time since last read timestamp = ros::Time::now(); - stopwatch_now = std::chrono::steady_clock::now(); - period.fromSec(std::chrono::duration_cast>(stopwatch_now - stopwatch_last).count()); - stopwatch_last = stopwatch_now; + period = kuka_rsi_hw_interface.getPeriod(); // Update the controllers controller_manager.update(timestamp, period); From 179ee18896bd74b1e5be2a7b8f91f262404fb4c9 Mon Sep 17 00:00:00 2001 From: Dennis Hartmann Date: Tue, 5 Jun 2018 15:44:24 +0200 Subject: [PATCH 3/3] Workaround for RSI 2.3 --- .../include/kuka_rsi_hw_interface/kuka_hardware_interface.h | 1 + kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp | 5 +++-- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.h b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.h index 2e520a408..92486283a 100644 --- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.h +++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.h @@ -110,6 +110,7 @@ class KukaHardwareInterface : public hardware_interface::RobotHW ros::Duration control_period_; ros::Duration elapsed_time_; double loop_hz_; + double krc_multiplier_; // Interfaces hardware_interface::JointStateInterface joint_state_interface_; diff --git a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp index 216f586da..83b674d2b 100644 --- a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp +++ b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp @@ -48,7 +48,7 @@ namespace kuka_rsi_hw_interface KukaHardwareInterface::KukaHardwareInterface() : joint_position_(6, 0.0), joint_velocity_(6, 0.0), joint_effort_(6, 0.0), joint_position_command_(6, 0.0), joint_velocity_command_( 6, 0.0), joint_effort_command_(6, 0.0), joint_names_(6), rsi_initial_joint_positions_(6, 0.0), rsi_joint_position_corrections_( - 6, 0.0), ipoc_(0), n_dof_(6) + 6, 0.0), ipoc_(0), n_dof_(6), krc_multiplier_(1.0) { in_buffer_.resize(1024); out_buffer_.resize(1024); @@ -106,7 +106,7 @@ bool KukaHardwareInterface::read(const ros::Time time, const ros::Duration perio rsi_state_ = RSIState(in_buffer_); if(rsi_state_.ipoc > ipoc_) { - control_period_.fromSec((rsi_state_.ipoc - ipoc_) / 1000.0); + control_period_.fromSec(krc_multiplier_ * (rsi_state_.ipoc - ipoc_) / 1000.0); } for (std::size_t i = 0; i < n_dof_; ++i) @@ -146,6 +146,7 @@ void KukaHardwareInterface::start() if (bytes < 100) { bytes = server_->recv(in_buffer_); + krc_multiplier_ = 12.0; } rsi_state_ = RSIState(in_buffer_);