How to achieve basic functionality?

[lcbw]

Ubuntu 22.04 | Branch: ros2 | UR10e & UR5e | ROS2 Humble | UR Polyscope 5.15 | headless = false

Hello, we are using your repository to learn the basic functionality of the FZI cartesian controllers and we haven't successfully made the robot move.

The modifications to code in this repository is minimal and can be found at our fork: https://github.com/lcbw/cartesian_controllers_universal_robots

We are currently testing this on a UR10e running polyscope 5.11. The on-pendant communication port is set to 50002, the robot IP is 172.31.1.137, and the computer IP is 172.31.1.17. We have updated the macro to pass in the reverse IP (computer IP) and can successfully connect to the robot and the current robot state appears in Rviz. We can successfully deactivate the scaled trajectory controller and activate any of the cartesian controllers. We can confirm that if we manually publish target_frames or target_wrenches from the command line, they appear as publishers on the /target_frame or /target_wrench topic respectively, and that the cartesian controller of interest is in fact subscribed to those topics.

These are the commands we are using to manually publish: ros2 topic pub /target_frame geometry_msgs/msg/PoseStamped "{header: {stamp: {sec: $(date +%s), nanosec: $(date +%N | cut -b1-9)}, frame_id: 'base_link'}, pose: {position: {x: 1.0, y: 0.5, z: 1.0}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}}" and ros2 topic pub /target_wrench geometry_msgs/msg/WrenchStamped "{header: {stamp: {sec: $(date +%s), nanosec: $(date +%N | cut -b1-9)}, frame_id: 'tool0'}, wrench: {force: {x: 10.0, y: 5.0, z: 0.0}, torque: {x: 0.0, y: 0.0, z: 1.0}}}" The robot does not move when we do these things.

Is there another step in operating the controllers that we are missing? Thank you.

[stefanscherzinger]

@lcbw

Ok, apart from the topics setup, which you seem to have managed, there are several other things to consider. Here's a list:

• The controllers will only accept topics when active

• There are some solver parameters that could slow things down: Could you check the p gains. They musn't be 0.0. I'm currently not sure whether they are if nothing is specified in the controller config. If the solver's error_scale is 0.0 then nothing will move. You can increase this parameter slowly with dynamic reconfigure to see where the robot is going. This is handy when publishing target_pose and that's more off than what you expect.

• A little off-topic but I stumbled over this at least once: The speed scaling of the UR teach pendant mustn't be set to 0. Speed scaling will negatively affect the controllers' performance, especially when in contact with force control.

• You might check whether you can move the robot by hand with pure force control, i.e. with the cartesian_force controller. I like doing that as a first check to confirm that everything behaves as expected. I then guide the robot into contact and adjust the error_scale until I'm happy with stability.

[lcbw]

Thank you for your help. Have you ever encountered a "C271A1: Low level real-time thread: Runtime is too much behind" error when trying to activate force or compliance controllers? I'm not finding very much documentation on this issue, and it's preventing me from activating the controllers today.

[stefanscherzinger]

No, I'm not really familiar with the C271A1. I only found your comment on the universal robots repo but cannot add any insight myself.

However, If not already done, try building the cartesian controllers in release mode. That should make the controller's algorithms notably faster.

[lcbw]

We have tried building the Cartesian controllers from source on branch ros2 in release mode and are continuing to encounter issues.
We are running into fewer C271A1 errors today, and are trying to push the robot around using the cartesian_force_controller.

We can: echo the /ft_sensor_wrench, publish a /target_wrench, and can see a sensible /ft_sensor_wrench in rviz. The we have tried tuning the P gains from 0.05 to 1000 and no movement has occurred when applying forces up to 60N on the robot. (That being said, we haven't E-Stopped the robot, so perhaps that means something. ) The robot is on, connected, and the controller is active. The "solver.publish_state_feedback" param is true and the solver.error_state is 0.5.

Are we missing anything? Thank you again --

[stefanscherzinger]

The we have tried tuning the P gains from 0.05 to 1000 and no movement has occurred when applying forces up to 60N on the robot.

Something's not connected properly. Careful with such high controller gains. That's several orders of magnitude too much. Could you post the controller's configuration (.yaml) for the force controller here? I'd like to see if I spot something unusual.

Also, just to be sure, could you post the output of

ros2 topic list | grep wrench

---

Edit: Have you ever made it work, and did it now stop working all of a sudden or has it never been working?

[lcbw]

Below are answers to your questions, but I did have a different thought @stefanscherzinger: is there any reason why trying this with two arms connected to the PC would be a problem? I'm only loading the driver for and controlling one arm at the moment, but the intention is to control 2.

cartesian controllers has worked on this particular UR10e before. The UR10e however was updated from 5.8 to 5.11 and cartesian controllers has not been tested on the updated system.

(The tf prefix of robot1 is confirmed with the system xacro)

cartesian_force_controller:
  ros__parameters:

    # See the cartesian_compliance_controller
    end_effector_link: "tool0"
    robot_base_link: "base_link"
    ft_sensor_ref_link: "tool0"
    joints:
      - shoulder_pan_joint
      - shoulder_lift_joint
      - elbow_joint
      - wrist_1_joint
      - wrist_2_joint
      - wrist_3_joint

    # See the cartesian_compliance_controller
    command_interfaces:
      - position
        #- velocity

    solver:
        error_scale: 0.5
        publish_state_feedback: True

    pd_gains:
        trans_x: {p: 0.5}
        trans_y: {p: 0.5}
        trans_z: {p: 0.5}
        rot_x: {p: 1.5}
        rot_y: {p: 1.5}
        rot_z: {p: 1.5}

ros2 topic list | grep wrench /ft_sensor_wrench /target_wrench