Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Use fixed period for update call of controller manager #132

Open
wants to merge 4 commits into
base: melodic-devel
Choose a base branch
from
Open

Use fixed period for update call of controller manager #132

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
c6dcb51
rsi_simulator now runs at fixed 250Hz and increases IPOC by 4
hartmanndennis Jun 4, 2018
6271a99
update controller manager with period taken from ipoc
hartmanndennis Jun 5, 2018
179ee18
Workaround for RSI 2.3
hartmanndennis Jun 5, 2018
9831f3d
Merge branch 'indigo-devel' into fixed_period
destogl Feb 25, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 2 additions & 3 deletions kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,8 +56,6 @@
#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/robot_hw.h>

// Timers
#include <chrono>

// UDP server
#include <kuka_rsi_hw_interface/udp_server.h>
Expand Down Expand Up @@ -112,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_;
Expand All @@ -126,7 +125,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
Expand Down
8 changes: 7 additions & 1 deletion kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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(krc_multiplier_ * (rsi_state_.ipoc - ipoc_) / 1000.0);
}

for (std::size_t i = 0; i < n_dof_; ++i)
{
joint_position_[i] = DEG2RAD * rsi_state_.positions[i];
Expand Down Expand Up @@ -141,6 +146,7 @@ void KukaHardwareInterface::start()
if (bytes < 100)
{
bytes = server_->recv(in_buffer_);
krc_multiplier_ = 12.0;
}

rsi_state_ = RSIState(in_buffer_);
Expand Down
9 changes: 1 addition & 8 deletions kuka_rsi_hw_interface/src/kuka_hardware_interface_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,18 +56,13 @@ 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);

kuka_rsi_hw_interface.start();

// 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<std::chrono::duration<double>>(stopwatch_now - stopwatch_last).count());
stopwatch_last = stopwatch_now;

// Run as fast as possible
while (ros::ok())
Expand All @@ -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<std::chrono::duration<double>>(stopwatch_now - stopwatch_last).count());
stopwatch_last = stopwatch_now;
period = kuka_rsi_hw_interface.getPeriod();

// Update the controllers
controller_manager.update(timestamp, period);
Expand Down
7 changes: 4 additions & 3 deletions kuka_rsi_simulator/scripts/kuka_rsi_simulator
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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:
Expand All @@ -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
Expand Down