ROS2 RVIZ Wheel Motion Discrepancy

[00Doc-T]

I'm running the ROS2 Control package, and have it plugged into a robot I'm developing. I'm using ODrive Botwheels, which should have encoder counts of 3200.

My model is scaled appropriately, and the wheel sizes are correct in my URDF.

The wheel is turning in RVIZ2 approximately 1/6 of the amount it is turning in reality. Has anyone else seen a similar issue.

There are two approaches that I can reason, to adjust things.

First would be to change the encoder counts per revolution in odrivetool setup.

The second would be to add a scaling factor into the hardware interface.

Am I missing something obvious here. Has anyone else seen a similar issue?

Thanks sincerely

-Chris

[anthonywebb]

Hi Chris. Funny you should mention it because we have seen this as well but have not had a chance yet to isolate or track it down. I also noticed that traveling 1 meter in the real world did not show a meter traveled in rviz. Probably should loop in @samuelsadok to see what might be off here.

[00Doc-T]

I checked:
odrv0.axis0.pos_estimate
Out[19]: 1.0148515701293945
This is showing that one revolution of the wheel (perfomed by hand), is resulting in one revolution in the odrive.

I also used ros2 topic echo /joint_states
Which showed that the wheel was moving exactly 1 rotation, when moved a complete rotation in joint states.

It's the TF frame that is the issue, which is odd.

[anthonywebb]

@samuelsadok is there a problem with the hardware interface causing this discrepancy?

[samuelsadok]

Since you say it's off by 1/6, it sounds a lot like an "off by 2*PI" error. ODrive typically uses turns and turns/s as units. I found it hard to find conclusive documentation on what ros2_control / ROS uses, and which part of the system defines it. But from what you say it sounds like it uses rad and rad/s.
Maybe you know where to find conclusive documentation? We could just add the factor in, but we'd also need to know what the right torque unit is.

Places that need to be changed:

ros_odrive/odrive_ros2_control/src/odrive_hardware_interface.cpp

Lines 288 to 290 in 72c2c79

	msg.Input_Pos = axis.pos_setpoint_;
	msg.Vel_FF = axis.vel_input_enabled_ ? axis.vel_setpoint_ : 0.0f;
	msg.Torque_FF = axis.torque_input_enabled_ ? axis.torque_setpoint_ : 0.0f;

(and below)

ros_odrive/odrive_ros2_control/src/odrive_hardware_interface.cpp

Lines 332 to 333 in 72c2c79

	pos_estimate_ = msg.Pos_Estimate;
	vel_estimate_ = msg.Vel_Estimate;

(and below)

[anthonywebb]

I’ve found mention around that it is indeed radians/second. I’ve asked Denis for confirmation and also asked about the torque units as well. I assume he will chime in here with more clarity @samuelsadok

[anthonywebb]

@samuelsadok i have found other diffdrive hardware interfaces that use radians for position and radians/second for velocity. All use nM for torque.

The other piece of this that is suspect is are we properly handling the conversion from rad/s back to rev/s (or m/s whatever the odrive expects for velocity) when we send the command back to the odrive?

[samuelsadok]

Thanks for digging into it. It seems there's really no definitive spec and we need to go by convention here.

I made a corresponding PR here: #17 but I don't have the hardware set up to test it. @00Doc-T if you can confirm that it fixes the discrepancy I will merge it.

@anthonywebb yes, both directions (ROS => ODrive / ODrive => ROS) are scaled in the PR.

[00Doc-T]

I can check on Friday or Saturday. I'm out of town until then.

…

-CT

On Tue, Apr 2, 2024, 10:25 AM samuelsadok ***@***.***> wrote: Thanks for digging into it. It seems there's really no definitive spec and we need to go by convention here. I made a corresponding PR here: #17 <#17> but I don't have the hardware set up to test it. @00Doc-T <https://github.com/00Doc-T> if you can confirm that it fixes the discrepancy I will merge it. @anthonywebb <https://github.com/anthonywebb> yes, both directions (ROS => ODrive / ODrive => ROS) are scaled in the PR. — Reply to this email directly, view it on GitHub <#15 (comment)>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/AQGLJXU6Q6RQUODKXA2MOZLY3KWTJAVCNFSM6AAAAABE2QLZ52VHI2DSMVQWIX3LMV43OSLTON2WKQ3PNVWWK3TUHMZDAMZSGAZTMMZQG4> . You are receiving this because you were mentioned.Message ID: ***@***.***>

[harborhoffer]

I can confirm this does indeed fix the discrepancy.

[samuelsadok]

ok, merged!

[00Doc-T]

I updated the files. Now motion is appropriately transformed in RVIZ. Has anyone noted that their motors are now underpowered pretty heavily?

[anthonywebb]

It was remarkable to me how little force I had to apply to stop the wheel from turning. I was trying to remember if I tested that before using the canbus control or not. But yes I feel like I could use some more spunk out of the motor. Probably need some more concrete tests.

[00Doc-T]

@samuelsadok

To follow this up, commenting out the 2pi changes in the sections regarding velocity, and leaving them in regards to position, along with changing the controllers.yaml file open loop to false fixed all issues. RVIZ has the correct positions, and the velocities are appropriate.

Code as below.

       if (axis.pos_input_enabled_) {
            Set_Input_Pos_msg_t msg;
            msg.Input_Pos = axis.pos_setpoint_ / (2 * M_PI);
            msg.Vel_FF = axis.vel_input_enabled_ ? (axis.vel_setpoint_   /*(2 * M_PI)*/) : 0.0f;
            msg.Torque_FF = axis.torque_input_enabled_ ? axis.torque_setpoint_ : 0.0f;
            axis.send(msg);
        } else if (axis.vel_input_enabled_) {
            Set_Input_Vel_msg_t msg;
            msg.Input_Vel = axis.vel_setpoint_ /*/ (2 * M_PI)*/;
            msg.Input_Torque_FF = axis.torque_input_enabled_ ? axis.torque_setpoint_ : 0.0f;
            axis.send(msg);
        } else if (axis.torque_input_enabled_) {
            Set_Input_Torque_msg_t msg;
            msg.Input_Torque = axis.torque_setpoint_;
            axis.send(msg);
        } else {
            // no control enabled - don't send any setpoint
        }
    }

    return return_type::OK;
}

void ODriveHardwareInterface::on_can_msg(const can_frame& frame) {
    for (auto& axis : axes_) {
        if ((frame.can_id >> 5) == axis.node_id_) {
            axis.on_can_msg(timestamp_, frame);
        }
    }
}

void Axis::on_can_msg(const rclcpp::Time&, const can_frame& frame) {
    uint8_t cmd = frame.can_id & 0x1f;

    auto try_decode = [&]<typename TMsg>(TMsg& msg) {
        if (frame.can_dlc < Get_Encoder_Estimates_msg_t::msg_length) {
            RCLCPP_WARN(rclcpp::get_logger("ODriveHardwareInterface"), "message %d too short", cmd);
            return false;
        }
        msg.decode_buf(frame.data);
        return true;
    };

    switch (cmd) {
        case Get_Encoder_Estimates_msg_t::cmd_id: {
            if (Get_Encoder_Estimates_msg_t msg; try_decode(msg)) {
                pos_estimate_ = msg.Pos_Estimate * (2 * M_PI);
                vel_estimate_ = msg.Vel_Estimate /** (2 * M_PI)*/;

    pose_covariance_diagonal : [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]
    twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]

    open_loop: false
    enable_odom_tf: true

    cmd_vel_timeout: 0.5

[samuelsadok]

Hmm this would contradict our earlier understanding that rad/s is the expected convention.

Do you mind sharing the launchfile (and additional files) that you're using to test this? Then I can see if I see any other issues.

And again, if you can refer to any documentation that defines the unit convention, that would also help.

[00Doc-T]

I uploaded a most recent copy of my code, and provided you access.

[samuelsadok]

Thanks! Given that the repo is private, can you hit me up on Discord or email ([email protected]) to let me know further details? E.g. which command to run and what to expect. If the issue can be seen without hardware that would be easiest cause I don't have a BotWheel Explorer near me, otherwise I would have to remote into one at another place. If you wanna demo the issue in a call that might also be an option.