-
Notifications
You must be signed in to change notification settings - Fork 118
Description
Describe the bug
Currently, the position command interface is implemented through proportional feedback of velocity, which means the actual behavior of any position command interface is essentially a first-order system. This is a bit odd (#87), but I think it's OK. What I want to raise here is another issue.
gz_ros2_control/gz_ros2_control/src/gz_system.cpp
Lines 655 to 662 in 2cde962
} else if (this->dataPtr->joints_[i].joint_control_method & POSITION) { | |
// Get error in position | |
double error; | |
error = (this->dataPtr->joints_[i].joint_position - | |
this->dataPtr->joints_[i].joint_position_cmd) * this->dataPtr->update_rate; | |
// Calculate target velcity | |
double target_vel = -this->dataPtr->position_proportional_gain_ * error; |
According to above code, update_rate
is introduced when calculating the velocity feedback, which seems questionable to me. If we analyze this system according to above code:
$x(t)$ : position
$c(t)$ : position command
$f$ : update_rate
$p$ : proportional gain
Apply the Laplace transform:
This means that the position command interface is a first-order system with a time constant
Based on the cart_example_position
demo, I did some tests. I removed the speed limit of the joint and sent step commands (ign topic -e -t /world/empty/dynamic_pose/info
) and got this image:
The curve in the figure perfectly fits the first-order system model above. And importantly, the system behaves differently depending on the update_rate
, which I don't think is a desirable behavior.
Expected behavior
The position command interface should have consistent behavior under different update_rate
. That means removing update_rate
from the above code.
In addition, it would be better if the time constant (or proportional feedback coefficient) of each position command interface can be configured separately.
Environment (please complete the following information):
- OS: Ubuntu 22.04
- Version: ROS2 Humble, Ignition Gazebo Fortress
Additional context
I can provide test code and data if needed.