FollowCartesianTrajectoryAction fails in a two-robot setup

[adkoessler]

Summary

I use a single computer to control two UR arms for synchronized motions. When calling the action server to execute a FollowCartesianTrajectoryAction on both robots, random outcomes appear: both robots stay still, only one moves while the other stays still, or both move.

Versions

• ROS Driver version: ROS Noetic for Ubuntu 20.04
• Affected Robot Software Version(s): Polyscope 5.11
• Affected Robot Hardware Version(s): both robots are UR10e
• UR+ product(s) installed: OnRobot 2FG7 gripper
• URCaps Software version(s): External Control 1.0.5

Impact

It is not possible to reliably execute synchronized motions (eg. for bi-arm manipulation)

Issue details

I am using a Python script inspired by test_move.py, where two FollowCartesianTrajectoryAction are instantiated in order to execute a coordinated motion:

def executeTrajectory(poseList1, poseList2, durationList):
	## Send trajectory to topic
	# using FollowCartesianTrajectoryAction
	goal1 = FollowCartesianTrajectoryGoal()
	trajectoryClient1 = actionlib.SimpleActionClient(
		"left_arm/joint_based_cartesian_traj_controller/follow_cartesian_trajectory",
		FollowCartesianTrajectoryAction,
	)
	goal2 = FollowCartesianTrajectoryGoal()
	trajectoryClient2 = actionlib.SimpleActionClient(
		"right_arm/joint_based_cartesian_traj_controller/follow_cartesian_trajectory",
		FollowCartesianTrajectoryAction,
	)
	for i, pose in enumerate(poseList1):
		point = CartesianTrajectoryPoint()
		point.pose = pose
		point.time_from_start = rospy.Duration(durationList[i])
		goal1.trajectory.points.append(point)
	for i, pose in enumerate(poseList2):
		point = CartesianTrajectoryPoint()
		point.pose = pose
		point.time_from_start = rospy.Duration(durationList[i])
		goal2.trajectory.points.append(point)
	confirmed = False
	valid = False
	while not valid:
		input_str = input(
		    "Please confirm that the robot path is clear of obstacles.\n"
		    "Keep the EM-Stop available at all times. You are executing\n"
		    "the motion at your own risk. Please type 'y' to proceed or 'n' to abort: "
		)
		valid = input_str in ["y", "n"]
		if not valid:
		    rospy.loginfo("Please confirm by entering 'y' or abort by entering 'n'")
		else:
		    confirmed = input_str == "y"
	if not confirmed:
		rospy.loginfo("Exiting as requested by user.")
		return 0
	rospy.loginfo(
		"Executing trajectory using the joint_based_cartesian_traj_controller"
	)
	trajectoryClient1.send_goal(goal1)
	trajectoryClient2.send_goal(goal2)
	trajectoryClient1.wait_for_result()
	trajectoryClient2.wait_for_result()
	result1 = trajectoryClient1.get_result()
	result2 = trajectoryClient2.get_result()

Launchfiles for both robots are namespaced and included in a single custom launchfile. After launching, the terminal looks like this:

[ INFO] [1652795383.764321387]: Robot mode is now RUNNING
[ INFO] [1652795383.765219434]: Robot mode is now RUNNING
[ INFO] [1652795383.765805193]: Robot's safety mode is now NORMAL
[ INFO] [1652795383.766806910]: Robot's safety mode is now NORMAL
[ INFO] [1652795391.886907468]: Robot requested program
[ INFO] [1652795391.886956837]: Sent program to robot
[ INFO] [1652795392.474397130]: Robot connected to reverse interface. Ready to receive control commands.
[ INFO] [1652795396.921303327]: Robot requested program
[ INFO] [1652795396.921388634]: Sent program to robot
[ INFO] [1652795397.316712691]: Robot connected to reverse interface. Ready to receive control commands.

In another terminal, services are called to use the joint_based_cartesian_traj_controller on both arms:

rosservice call /left_arm/ur_hardware_interface/dashboard/play
rosservice call /right_arm/ur_hardware_interface/dashboard/play
rosservice call /left_arm/controller_manager/switch_controller "start_controllers: []  
stop_controllers: ['scaled_pos_joint_traj_controller']
strictness: 2
start_asap: false
timeout: 0.0"
rosservice call /right_arm/controller_manager/switch_controller "start_controllers: []                                    
stop_controllers: ['scaled_pos_joint_traj_controller']
strictness: 2
start_asap: false
timeout: 0.0"
rosservice call /left_arm/controller_manager/switch_controller "start_controllers: ['joint_based_cartesian_traj_controller']
stop_controllers: []                                  
strictness: 2
start_asap: false
timeout: 0.0"
rosservice call /right_arm/controller_manager/switch_controller "start_controllers: ['joint_based_cartesian_traj_controller']
stop_controllers: []                                  
strictness: 2
start_asap: false
timeout: 0.0"

The script is executed in a 3rd terminal:

~/ur_workspace$ rosrun deformation_controllers robot_2grasp_trajectory_sender
Please confirm that the robot path is clear of obstacles.
Keep the EM-Stop available at all times. You are executing
the motion at your own risk. Please type 'y' to proceed or 'n' to abort: y
[INFO] [1652795492.572792]: Executing trajectory using the joint_based_cartesian_traj_controller

However, the robots have not moved and the terminal where the driver nodes run displays:

[ INFO] [1652795492.601901827]: Connection to reverse interface dropped.
[ INFO] [1652795492.602761934]: Robot connected to reverse interface. Ready to receive control commands.
[ INFO] [1652795492.604204538]: Connection to reverse interface dropped.
[ INFO] [1652795492.604724043]: Robot connected to reverse interface. Ready to receive control commands.

Important remarks:

• I have been able to successfully execute synchronized motions using the twist_controller in the past. TF prefixes are used and joints have been renamed to differentiate between both arms.
• test_move.py seems to work flawlessly on each robot independently
• I have always used the same trajectory for all the tests.
• I have plenty of nodes to run before executing the script which calls the FollowCartesianTrajectoryAction. Maybe there can be some sort of timeout coming into play?

Use case

Please see video: https://www.youtube.com/watch?v=V6lFX4VA86Y

Expected Behavior

Each robot should performed the intended trajectory once the goal is sent by the action client.

Actual Behavior

Sometimes both arms execute the trajectory, sometimes one does while the other stands still, sometimes both stand still.

[fmauch]

I assume that running two drivers on one machine / network interface could be too much, but that's just a very brief assumtion.

However, if you want to use truely synchronized motions, I suggest you use a combined_robot_hw approach where both robots will be inside the same hardware interface for that. This driver's hardware interface should be equipped to allow using this rather easily. As you obviously are aware of avoiding joint name collisions this should be rather quick to setup.

[adkoessler]

Thank you @fmauch and sorry for the late reply.

I am trying to follow the combined_robot_hw tutorial, but creating a 'ur_robot_driver/config/combo_ur10e_controllers.yaml` file for controllers raises two questions:

1. In two following piece of code, how can it be ensured that left_joint_state_controller, left_force_torque_sensor_controller and left_speed_scaling_state_controller really point towards &left_robot_joints?

comboRobot:
    # Settings for ros_control control loop
    hardware_control_loop:
       loop_hz: &loop_hz 500

    leftRobot:
        # Settings for ros_control hardware interface
        ur_hardware_interface:
           joints: &left_robot_joints
             - left_arm_shoulder_pan_joint
             - left_arm_shoulder_lift_joint
             - left_arm_elbow_joint
             - left_arm_wrist_1_joint
             - left_arm_wrist_2_joint
             - left_arm_wrist_3_joint
    
    rightRobot:
        # Settings for ros_control hardware interface
        ur_hardware_interface:
           joints: &right_robot_joints
             - right_arm_shoulder_pan_joint
             - right_arm_shoulder_lift_joint
             - right_arm_elbow_joint
             - right_arm_wrist_1_joint
             - right_arm_wrist_2_joint
             - right_arm_wrist_3_joint

    controller:
        # Publish all joint states ----------------------------------
        left_joint_state_controller:
           type:         joint_state_controller/JointStateController
           publish_rate: *loop_hz

        # Publish wrench ----------------------------------
        left_force_torque_sensor_controller:
           type:         force_torque_sensor_controller/ForceTorqueSensorController
           publish_rate: *loop_hz

        # Publish speed_scaling factor
        left_speed_scaling_state_controller:
           type:         scaled_controllers/SpeedScalingStateController
           publish_rate: *loop_hz
        
        # Publish all joint states ----------------------------------
        right_joint_state_controller:
           type:         joint_state_controller/JointStateController
           publish_rate: *loop_hz

        # Publish wrench ----------------------------------
        right_force_torque_sensor_controller:
           type:         force_torque_sensor_controller/ForceTorqueSensorController
           publish_rate: *loop_hz

        # Publish speed_scaling factor
        right_speed_scaling_state_controller:
           type:         scaled_controllers/SpeedScalingStateController
           publish_rate: *loop_hz

2. In the case of controllers for Cartesian pose and twist, how should parameters of the combined versions be defined? Is it even possible to have combined versions for such controllers? For instance with the joint_based_cartesian_traj_controller, setting up parameters base and tip is not obvious when dealing with two robots.

Best, Adrien

[fmauch]

As for your first question: That is indeed something I did not think of. You will, most probably, get an error on startup as you try to register the "speed_scaling_factor" twice. Apart from this, a question would be what would actually be the desired behavior. In my opinion it would not make much sense to have different speed scaling values at all, as this would be going against the desired behavior of a synchronized motion. Sorry that I set you on a path not leading towards your desired goal. You could play around by adding the tf_prefix to the handle's name to avoid the name collision, but I am still not sure about the behavior.

Regarding your second question: No, this is not possible. The Cartesian controllers are designed to move one frame through space. You might be able to workaround this using a virtual frame attached with a joint to both TCPs in the URDF, but I am not sure whether this will actually work...

Edit: For modifying the handle's name, you could also try #544

[github-actions[bot]]

This issue has not been updated for a long time. If no further updates are added, this will be closed automatically. Comment on the issue to prevent automatic closing.

[github-actions[bot]]

This issue has been closed due to inactivity. Feel free to comment or reopen if this is still relevant.