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

New: Calculate elapsed time in ctrl_loop #92

Merged
merged 1 commit into from
Nov 29, 2022
+20 −19
Merged

New: Calculate elapsed time in ctrl_loop #92

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
4 changes: 2 additions & 2 deletions iiwa_driver/include/iiwa_driver/iiwa.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -87,8 +87,8 @@ namespace iiwa_ros {
void _init();
void _ctrl_loop();
void _load_params();
void _read(ros::Duration elapsed_time);
void _write(ros::Duration elapsed_time);
void _read(const ros::Duration& ctrl_duration);
void _write(const ros::Duration& ctrl_duration);
bool _init_fri();
bool _connect_fri();
void _disconnect_fri();
Expand Down
35 changes: 18 additions & 17 deletions iiwa_driver/src/iiwa.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -246,16 +246,17 @@ namespace iiwa_ros {

void Iiwa::_ctrl_loop()
{
ros::Time time = ros::Time::now();
const ros::Duration ctrl_duration = ros::Duration(1. / _control_freq);
// Time since the last call of update
ros::Duration elapsed_time;
while (ros::ok()) {
ros::Time time = ros::Time::now();

// TO-DO: Get real elapsed time?
auto elapsed_time = ros::Duration(1. / _control_freq);

_read(elapsed_time);
// Read is blocking until FRI has replied
_read(ctrl_duration);
elapsed_time = ros::Time::now() - time;
time = ros::Time::now();
_controller_manager->update(ros::Time::now(), elapsed_time);
_write(elapsed_time);

_write(ctrl_duration);
_publish();
}
}
Expand Down Expand Up @@ -292,7 +293,7 @@ namespace iiwa_ros {
n_p.getParam("hardware_interface/joints", _joint_names);
}

void Iiwa::_read(ros::Duration elapsed_time)
void Iiwa::_read(const ros::Duration& ctrl_duration)
{
// Read data from robot (via FRI)
kuka::fri::ESessionState fri_state;
Expand Down Expand Up @@ -321,23 +322,23 @@ namespace iiwa_ros {

for (int i = 0; i < _num_joints; i++) {
_joint_position[i] = _robot_state.getMeasuredJointPosition()[i];
_joint_velocity[i] = filters::exponentialSmoothing((_joint_position[i] - _joint_position_prev[i]) / elapsed_time.toSec(), _joint_velocity[i], 0.2);
_joint_velocity[i] = filters::exponentialSmoothing((_joint_position[i] - _joint_position_prev[i]) / ctrl_duration.toSec(), _joint_velocity[i], 0.2);
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why not elapsed_time here? I guess you have good reasons, but I'd like to have them documented.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

well, one of them is that we should handle interruptions then. That leads to very large values for the measured elapsed time. Like seconds.

The second reason is that when we implemented the controller for the IROS 2021 paper, you told me that we should not use the actual time when integrating, but only the expected time. I applied the same reasoning on the derivative here.

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Good enough! Merging... Thanks again! :)

_joint_effort[i] = _robot_state.getMeasuredTorque()[i];
}
}

void Iiwa::_write(ros::Duration elapsed_time)
void Iiwa::_write(const ros::Duration& ctrl_duration)
{
if (_idle) // if idle, do nothing
return;

// enforce limits
_position_joint_limits_interface.enforceLimits(elapsed_time);
_position_joint_saturation_interface.enforceLimits(elapsed_time);
_effort_joint_limits_interface.enforceLimits(elapsed_time);
_effort_joint_saturation_interface.enforceLimits(elapsed_time);
_velocity_joint_limits_interface.enforceLimits(elapsed_time);
_velocity_joint_saturation_interface.enforceLimits(elapsed_time);
_position_joint_limits_interface.enforceLimits(ctrl_duration);
_position_joint_saturation_interface.enforceLimits(ctrl_duration);
_effort_joint_limits_interface.enforceLimits(ctrl_duration);
_effort_joint_saturation_interface.enforceLimits(ctrl_duration);
_velocity_joint_limits_interface.enforceLimits(ctrl_duration);
_velocity_joint_saturation_interface.enforceLimits(ctrl_duration);

// reset commmand message
_fri_message_data->resetCommandMessage();
Expand Down