Robot shakes on command mode switch

[caioaamaral]

Describe the bug
JointTrajectoryController causes robot to "jitter" on activation if joint was previous moved by another controller

To Reproduce
Steps to reproduce the behavior:

1. Move joints using a different controller (for instance I'm using velocity_controllers/JointGroupVelocityController)
2. Deactivate the first controller and activate JointTrajectoryController

Expected behavior
I expect the robot to keep its current position, since no command was send

Screenshots

dashboard_bug.mp4

Environment (please complete the following information):

• OS: Ubuntu 22.04
• Version [Humble]
• JointTrajectoryController is configure with:
  • set_last_command_interface_value_as_state_on_activation: false
  • open_loop_control: false

Additional context
JTC is configure in a prismatic joint

[caioaamaral]

here a different view, when I activate JointTrajectoryController and send a command

bug2.webm

[christophfroehlich]

It seems that this is fixed with ros-controls/ros2_controllers#1231
set set_last_command_interface_value_as_state_on_activation to false

[caioaamaral]

set_last_command_interface_value_as_state_on_activation

@christophfroehlich I'm already using it as false. When it is true, the joint moves all the way to the previous position, but when is false I just see this slight shake

[christophfroehlich]

can you plot the controller_state topic? maybe this is just an interpolation issue

[caioaamaral]

@christophfroehlich (is it a problem tagging you?) I've attempt to record as many fields as possible within controller_state_topic

dashboard_bug_02.mp4

[christophfroehlich]

set_last_command_interface_value_as_state_on_activation

@christophfroehlich I'm already using it as false. When it is true, the joint moves all the way to the previous position, but when is false I just see this slight shake

Sorry, I haven't read that in your initial message 🙈

@christophfroehlich (is it a problem tagging you?) I've attempt to record as many fields possible within controller_state_topic

No need to tag, I will answer without it too.

Can you please plot the position/velocity of one joint where you observe the jump? Is this a gazebo simulation?

[caioaamaral]

Is this a gazebo simulation?

yes, Gazebo Fortress with ign_ros2_control

Can you please plot the position/velocity of one joint where you observe the jump?

from which topic precisily, /control_state/error, /control_state/feedback?

[Edit] I did notice that feedback/positions[0] and error/positions[0] have a slight change

[caioaamaral]

with open_loop_control: true it the behavior is more violent

[christophfroehlich]

There are too many lines in the plot, sorry 😬 From the topic monitor above it seems that you use position only? then you can skip the velocites from the plot. This also means that you have no PID loops, this is just feedforward control?
set_hold_position() takes the value from state_current_, which is the state from the hardware with your parameters. I don't know where the jump should come from.
open_loop on humble changes the behavior of setting the point before the actual trajectory. with the hold position this should not make much difference (I thought).

[caioaamaral]

There are too many lines in the plot, sorry

I was unsure on which fields you would be interested

This also means that you have no PID loops, this is just feedforward control?

I do not specify pid in the follow joint trajectory goal. This is how it's configured:

left_arm_position_controller:
  ros__parameters:
    joints:
      - left_arm_motor

    command_interfaces:
      - position
    state_interfaces:
      - position
    set_last_command_interface_value_as_state_on_activation: false
    open_loop_control: false

set_hold_position() takes the value from state_current_, which is the state from the hardware with your parameters

it seems there is a slight variance beteween the actual position and the feedback position right at the start

[EDIT] I've captured joint states as well

[christophfroehlich]

the output position remains constant in the first plot -> this is not the fault of the controller. It seems that it just takes the correct value before the activation (considering the joint_states).
How can i understand your robot, there are multiple links moving in the video. Do you have any closed-loop kinematics? What happens if you just deactivate the controller without activating a different one: Does the robot move? (fall down..)

[caioaamaral]

How can i understand your robot, there are multiple links moving in the video

each arm (left and right) has a independent JointTrajectoryController defined against revolute joints, each controller has a single joint (as you can see on my previous comment)

Do you have any closed-loop kinematics?

Nop, there is nothing else I'm aware controlling the arms, the only controllers defined for this joint is the Homing (JointGroupVelocity) and the Position (JointTrajectoryController)

What happens if you just deactivate the controller without activating a different one: Does the robot move? (fall down..)

It just stays on position, there is no external forces on it. If for instance I activate just the controller for the right arm, left stays in position and right do shakes a bit.

the output position remains constant in the first plot -> this is not the fault of the controller.

Do you recomend any further debugging? What I know is that set_last_command_interface_value_as_state_on_activation and open_loop_control amplifies the behavior we are seeing. Could it be a remaining velocity command from the previous controller (besides it being inactive, it uses the same hardware component)?

[christophfroehlich]

Do you recomend any further debugging? What I know is that set_last_command_interface_value_as_state_on_activation and open_loop_control amplifies the behavior we are seeing. Could it be a remaining velocity command from the previous controller (besides it being inactive, it uses the same hardware component)?

This might be an issue, because we don't reset the commands in the perform_command_mode switch method. Can you try to send zero velocity before you switch the controllers?

[caioaamaral]

Tested today with real motors, with set_last_command_interface_value_as_state_on_activation and open_loop_control set as false. It works normally, without the shake.

So I believe there is something happening at gazebo domain

Can you try to send zero velocity before you switch the controllers?

On gazebo, I set my velocity controller command_interfaces_[0].set_value(0.0); on_deactivate:

controller_interface::CallbackReturn HomingController::on_deactivate(const rclcpp_lifecycle::State & previous_state)
{
  auto ret = ForwardCommandController::on_deactivate(previous_state);
  if (ret != CallbackReturn::SUCCESS) {
    return ret;
  }

  controller_active_ = false;

  if (goal_handle_ptr_) {
    RCLCPP_INFO(get_node()->get_logger(), "HOMING GOAL ABORTED, CONTROLLER IS DEACTIVATED");

    this->abort_homing(*goal_handle_ptr_);
    goal_handle_ptr_ = nullptr;
    homing_in_progress_ = false;
  }

  command_interfaces_[0].set_value(0.0);

  return CallbackReturn::SUCCESS;
}

controller_interface::return_type HomingController::update(
  const rclcpp::Time & time, const rclcpp::Duration & period)
{
  if (!homing_in_progress_) {
    command_interfaces_[0].set_value(0.0);
    return controller_interface::return_type::OK;
  }

  ...
}

My velocity controller is a child from ForwardCommandController (not JointGroupVelocityController as I previous said)

I believe this is enough to set the command_interface to zero, am I right? Even dough I still see the shake behavior on simulation