Initial positions are set to 0 causing sudden movement

[daniel-kovari]

Describe the feature you want

I am using ROS Jazzy and I do not think the arm motors should start with position 0 on_activate. This causes a jerky motion on_activate where the first command moves it to position 0 suddenly. When the system starts, it should read each motor’s actual position and set the command targets to match and then send a command to move to 0s.

[euyniy]

I think it does make sense to have on_activate to explicitly update the states. In terms of returning to zero, I think perhaps remove it completely could be better.
By the way, can you tell me more about when do jerky movements occur? Is the controller activated when the arm is disabled but at a very unnatural pose (like resting on a table?)

[daniel-kovari]

Yes, I am running ROS2 Jazzy and using the v10_simple_hardware as my hardware system interface. I am communicating via CAN2.0 in MIT control mode. When I send my first bringup command:

    openarm_left_joint1: -0.15
    openarm_left_joint2: 0.0
    openarm_left_joint3: 0.0
    openarm_left_joint4: 0.9
    openarm_left_joint5: 0.0
    openarm_left_joint6: 0.0
    openarm_left_joint7: 0.0

    openarm_right_joint1: 0.15
    openarm_right_joint2: 0.0
    openarm_right_joint3: 0.0
    openarm_right_joint4: 0.9
    openarm_right_joint5: 0.0
    openarm_right_joint6: 0.0
    openarm_right_joint7: 0.0

The arms jerk to 0s before smoothly reaching this position. I believe that this happens because, on the very first MIT control cycle, both pos_commands_ and pos_states_ are initialized to 0.0. As a result, the controller sends a command that drives every joint toward zero before updating with the actual desired positions. If we set those to what the motors currently are, then we avoid this jerky motion.

I removed the return_to_zero(); call in my on_activate function but left it in the pull request to match the previous behavior.

[aa323148]

是的，我正在運行 ROS2 Jazzy 版本，並使用 v10_simple_hardware 作為我的硬體系統介面。我透過 CAN2.0 以 MIT 控制模式進行通訊。當我發送第一個啟動命令時：

    openarm_left_joint1: -0.15
    openarm_left_joint2: 0.0
    openarm_left_joint3: 0.0
    openarm_left_joint4: 0.9
    openarm_left_joint5: 0.0
    openarm_left_joint6: 0.0
    openarm_left_joint7: 0.0

    openarm_right_joint1: 0.15
    openarm_right_joint2: 0.0
    openarm_right_joint3: 0.0
    openarm_right_joint4: 0.9
    openarm_right_joint5: 0.0
    openarm_right_joint6: 0.0
    openarm_right_joint7: 0.0

機械手臂在平穩到達目標位置之前會突然跳動到 0。我認為這是因為在第一個 MIT 控制週期中，pos_commands_ 和 pos_states_ 都被初始化為 0.0。因此，控制器會發送一個指令，使每個關節先向零點移動，然後再更新為實際的目標位置。如果我們把這些值設定為馬達目前的實際位置，就可以避免這種跳動。

我在 on_activate 函數中移除了 return_to_zero(); 調用，但在 pull request 中保留了它，以與先前的行為保持一致。

[aa323148]

Even after removing the return_to_zero(); call from the on_activate function, it still reverts to 0. I've tried everything, but nothing works!