With joint_trajectory_controller, sending a new command causes the torque to momentarily relax

[daipom]

Describe the problem you got

With joint_trajectory_controller, sending a new command causes the torque to momentarily relax.
As a result, the arm briefly drops.
It seems to drop more when the gain is small.

#72 (comment)

i have the same problem i am using humble didn't change anything in the code or config when next command goes it drops and suddenly tries to go to the position

WhatsApp.Video.2026-01-04.at.6.02.46.PM.mp4

Cause

This is caused by poor compatibility with the joint_trajectory_controller.
It does not occur when using the forward_position_controller in openarm_bringup.

The exact root cause is unknown, but this seems to be triggered when the next action is close to the current position.
joint_trajectory_controller interpolates the trajectory, so the first point is always very close to the current position.
This likely causes this behavior.

Reproduce

1. ros2 launch openarm_bimanual_moveit_config demo.launch.py
2. In RViz, set -30 to openarm_left_joint1 in MotionPlanning - Joints, and run Plan & Execute in MotionPlanning - Planning.
3. In RViz, set -45 to openarm_left_joint1 in MotionPlanning - Joints, and run Plan & Execute in MotionPlanning - Planning.
4. You can confirm the left arm briefly drops.

trim.D3054492-B45B-4DD2-A8ED-B904E5509334.MOV

forward_position_controller

This does not occur with forward_position_controller.

1. ros2 launch openarm_bringup openarm.launch.py robot_controller:=forward_position_controller
2. ros2 topic pub /forward_position_controller/commands std_msgs/msg/Float64MultiArray '{data: [-0.10, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]}'
3. ros2 topic pub /forward_position_controller/commands std_msgs/msg/Float64MultiArray '{data: [-0.20, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]}'
4. ros2 topic pub /forward_position_controller/commands std_msgs/msg/Float64MultiArray '{data: [-0.30, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]}'

[ggorjup]

I think this is caused by joints not reaching their target positions.

When moving joint1 with the joint_trajectory_controller:

1. We set the target to -30 deg, plan, and execute. The controller interpolates from the current position (0 deg) to the target, but it does not reach it. Due to arm weight, the joint stops at e.g. -25 deg, depending on set MIT gains.
2. We set the target to -45 deg, plan, and execute. MoveIt produces a plan from the current joint state (-25 deg) to the target state (-45 deg). However, the robot ended up at -25 deg because we commanded -30 deg in the previous move: when commanding -25 deg now, the robot moves even lower (e.g. -21 deg), which looks like a drop in torque. The plan then continues from the commanded -25 deg to the commanded -45 deg, finally ending up at e.g. -38 deg.

When moving joint1 with the forward_position_controller:

1. We set the target to -30 deg, but the joint does not reach it.
2. We then set the target to -45 deg, and the joint moves to e.g. -38 deg. The arm does not drop because we only updated the commanded position. However, the difference between the commanded and actual joint position is in both cases several degrees.

Below are the target and reached arm position when commanding joint1 to go to -30 deg (gains wrt. eeca95e). The difference is roughly 5 degrees:

[daipom]

Thanks for your report!

I think this is caused by joints not reaching their target positions.

We set the target to -45 deg, plan, and execute. MoveIt produces a plan from the current joint state (-25 deg) to the target state (-45 deg). However, the robot ended up at -25 deg because we commanded -30 deg in the previous move: when commanding -25 deg now, the robot moves even lower (e.g. -21 deg), which looks like a drop in torque.

It seems like a possible explanation.
However, I’ve seen a similar behavior using a different setup from ROS2 and MoveIt, and in that case, I confirmed from the data that the torque actually dropped momentarily.
So it might be worth checking whether torque is really not the cause this time as well.

[daipom]

I have confirmed that the torque(effort) actually dropped momentarily.
So, it appears the cause is the torque relax.

• ros2 launch openarm_bimanual_moveit_config demo.launch.py
• openarm_left_joint1: 0 -> -15 -> -30 -> -45 -> -60

Node for recording CSV.

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from control_msgs.msg import DynamicJointState

TARGET_JOINT = "openarm_left_joint1"

class OneJointPrinter(Node):
    def __init__(self):
        super().__init__("one_joint_printer")
        self.sub = self.create_subscription(
            DynamicJointState,
            "/dynamic_joint_states",
            self.cb,
            10,
        )

    def cb(self, msg: DynamicJointState):
        timestamp = f"{msg.header.stamp.sec}.{msg.header.stamp.nanosec}"

        joints = list(msg.joint_names)
        if TARGET_JOINT not in joints:
            print(f"{TARGET_JOINT} not in joint_names.")
            return

        iv = msg.interface_values[joints.index(TARGET_JOINT)]
        iface_map = {}
        for name, value in zip(iv.interface_names, iv.values):
            iface_map[name] = value

        data = [timestamp, *iface_map.values()]
        print(",".join(map(str, data)))

def main():
    rclpy.init()
    node = OneJointPrinter()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == "__main__":
    main()

Data: out3.csv

[MuXingchui]

It need automatic gravity compensation. Maybe need integral of PID also.

[micondy]

I used openarm_teleop src/controller/dynamics from this repository.
After adding it into v10_simple_hardware.cpp, everything runs very well now.