Skip to content

Position command interface behaves differently depending on the update_rate #611

@Narukara

Description

@Narukara

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.

} 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

$$ e = (x-c)f $$ $$ v = \dot x = -pe = -p(x-c)f $$ $$ \dot x + fpx = fpc $$

Apply the Laplace transform:

$$ sX(s) + fpX(s) = fpC(s) $$ $$ \frac{X(s)}{C(s)} = \frac{1}{(1/fp)s+1}$$

This means that the position command interface is a first-order system with a time constant $T$ of $1/fp$. The problem is that the position command interface should represent some kind of hardware. Its characteristics, such as time constants, should only be related to the hardware itself. The controller update rate $f$ should not appear here.

Based on the cart_example_position demo, I did some tests. I removed the speed limit of the joint and sent step commands ($c=10$) at different controller manager update rate. Then I collected data from gazebo (ign topic -e -t /world/empty/dynamic_pose/info) and got this image:

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.

Metadata

Metadata

Assignees

No one assigned

    Labels

    bugSomething isn't working

    Type

    No type

    Projects

    No projects

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions