Joint Trajectory Server times out even after reaching goal pose when wrist is under initial force

[hello-amal]

Description

Consider the case where we send the JointTrajectoryServer a trajectory with multiple joints (e.g., all arm, wrist, and head joints). The action should move to those joints and succeed. (And, as a corollary, if the joints are already at those positions, the trajectory should immediately succeed). This happens when the wrist is not under any external force at the beginning of the action call. However, if the wrist is under some initial external force (e.g., object in its gripper, a hand lightly pulling it down, etc.), then the action server calls time out and fail.

(Note that this issue does arise in real-world usage, e.g., when using the web app's Movement Recorder while the robot is holding an object.)

Steps to re-create

1. Copy the below code to a ROS2 package and update the CMakeLists.txt/setup.py file to compile it as a node.
2. Launch the driver: ros2 launch stretch_core stretch_driver.launch.py
3. Run the node once. ros2 run <pkg-name> move_to_same_pose.py. Verify that all commanded trajectories successfully complete in a timely fashion.
4. Manually place an object in the gripper (I tested with my water bottle, which is a bit larger than the commanded gripper aperture, and a tennis ball).
5. Re-run it. Verify that even after moving to the target goal, the action server still doesn't succeed until timeout.
   1. (Note, although putting an object in the gripper is the most reliable way to re-create the issue, I've also been able to re-create it by putting my hand below the gripper and exerting a small force pulling down.)

move_to_same_pose.py

#! /usr/bin/env python3
import threading
from typing import Dict, Optional

import rclpy
from action_msgs.msg import GoalStatus
from control_msgs.action import FollowJointTrajectory
from rclpy.action import ActionClient
from rclpy.duration import Duration
from rclpy.executors import SingleThreadedExecutor
from rclpy.node import Node
from rclpy.task import Future
from trajectory_msgs.msg import JointTrajectoryPoint

# Declare the global variables
node: Optional[Node] = None
trajectory_client: Optional[ActionClient] = None


def move_to(joint_positions: Dict[str, float]) -> Future:
    """
    Move the arm to the given joint positions.

    Parameters
    ----------
    joint_positions : Dict[str, float]
        A dictionary of joint names and their corresponding positions.

    Returns
    -------
    Future
        A future object representing the result of the action.
    """
    global node, trajectory_client
    joint_names = list(joint_positions.keys())

    point1 = JointTrajectoryPoint()
    trajectory_goal = FollowJointTrajectory.Goal()
    trajectory_goal.goal_time_tolerance = Duration(seconds=1.0).to_msg()
    trajectory_goal.trajectory.joint_names = joint_names

    point1.time_from_start = Duration(seconds=10).to_msg()
    point1.positions = [joint_positions[joint_name] for joint_name in joint_names]
    trajectory_goal.trajectory.points = [point1]
    return trajectory_client.send_goal_async(trajectory_goal)


def wait_until_future_finished(future: Future, rate_hz: float = 5.0) -> None:
    """
    Wait until the future is finished.

    Parameters
    ----------
    future : Future
        The future object to wait for.
    rate_hz : float, optional
        The rate at which to check the future, by default 5.0
    """
    global node
    rate = node.create_rate(rate_hz)
    while not future.done():
        rate.sleep()


def get_action_result(
    send_goal_future: Future,
) -> Optional[FollowJointTrajectory.Result]:
    """
    Get the result of the action.

    Parameters
    ----------
    send_goal_future : Future
        The future object representing the result of the action.

    Returns
    -------
    Optional[FollowJointTrajectory.Result]
        The result of the action, if available.
    """
    global node
    # First, wait until the goal is accepted or rejected
    wait_until_future_finished(send_goal_future)

    # Check if the goal was accepted
    goal_handle = send_goal_future.result()
    if not goal_handle.accepted:
        node.get_logger().error("Goal rejected")
        return None

    # Wait until the goal is completed
    get_result_future = goal_handle.get_result_async()
    wait_until_future_finished(get_result_future)

    # Return the result
    goal_status = get_result_future.result()
    if goal_status.status != GoalStatus.STATUS_SUCCEEDED:
        node.get_logger().error("Goal failed")
        return None
    return goal_status.result


def main():
    global node, trajectory_client
    rclpy.init()
    node = rclpy.create_node("move_to_same_pose")
    trajectory_client = ActionClient(
        node, FollowJointTrajectory, "/stretch_controller/follow_joint_trajectory"
    )
    node.get_logger().info("Waiting for the follow joint trajectory controller...")
    _ = trajectory_client.wait_for_server(timeout_sec=60.0)

    # Spin the node in the background
    executor = SingleThreadedExecutor()
    spin_thread = threading.Thread(
        target=rclpy.spin,
        args=(node,),
        kwargs={"executor": executor},
        daemon=True,
    )
    spin_thread.start()

    # Create the goal
    goals = [
        {
            "joint_head_tilt": -0.02766916597432903,
            "joint_head_pan": 0.050683455439404626,
            "joint_wrist_roll": -0.006135923151542565,
            "joint_wrist_pitch": -0.15339807878856412,
            "joint_wrist_yaw": 0.0012783173232380344,
            "joint_gripper_finger_left": 0.325355908608982,
            "joint_lift": 0.5305833379477396,
            "wrist_extension": 0.07205661388208048,
        },
        {
            "joint_head_tilt": -0.03766916597432903,
            "joint_head_pan": 0.060683455439404626,
            "joint_wrist_roll": -0.016135923151542565,
            "joint_wrist_pitch": -0.16339807878856412,
            "joint_wrist_yaw": 0.0112783173232380344,
            "joint_gripper_finger_left": 0.325355908608982,
            "joint_lift": 0.6305833379477396,
            "wrist_extension": 0.17205661388208048,
        },
    ]

    # Move to the goal 3 times
    for goal in goals:
        for _ in range(3):
            node.get_logger().info(f"Moving to {goal}")
            send_goal_future = move_to(goal)
            node.get_logger().info("Waiting to complete execution...")
            get_action_result(send_goal_future)
            node.get_logger().info("Completed execution")

    # Spin in the foreground
    spin_thread.join()

    # Clean up
    node.destroy_node()
    rclpy.shutdown()


if __name__ == "__main__":
    main()

[hello-amal]

@hello-binit , after you resolve #140 , it would be great if you could take a look at this. Because I'm seeing this a lot when toggling the tablet to portrait mode, which means that after we toggle the tablet to portrait mode, we can't execute any wrist roll trajectories for 10s until that trajectory goal times out.

[hello-amal]

EDIT:

after we toggle the tablet to portrait mode, we can't execute any wrist roll trajectories for 10s until that trajectory goal times out.

The above was actually caused by the order in which ROS was receiving cancel commands vs. actual trajectory commands.

To be clear, the overall bug (first message in this issue) is still a problem, but there is a workaround which is to just cancel the goal (or send another goal) since the joint trajectory server handles that well.

[hello-amal]

I have certified that the underlying cause of this issue is what is highlighted in #151 . I'm keeping this open because it has a useful example to re-create it with the Joint Trajectory Server (whereas #151 only has a stretch_body example).