Path tolerance violated when using FollowJointTrajectoryActionGoal and the ur_robot_driver with UrSim

[sharpe-developer]

Summary

Path tolerance violated when using moveit and the ur_robot_driver with UrSim

I am using moveit to path plan. Plans generated by moveit run perfectly on the UR3e in RViz and Gazebo. However, when I attempt to execute the same path on the UrSim simulated UR3 the controller fails and aborts.

I have been stuck on this for a while and have not found a solution. Any assistance will be greatly appreciated.

Versions

• ROS Driver version: 0.0.3
• Affected Robot Software Version(s): ursim-5.7.0.90932
• URCaps Software version(s): externalcontrol-1.0.1

Impact

Cannot utilize the physical robot for tasks

Issue details

Plans generated by moveit run perfectly on the UR3e in RViz and Gazebo. When I attempt to execute the same path on the UrSim simulated UR3 the controller fails and aborts. I receive the following messages for each move:

ABORTED: Solution found but controller failed during execution

[ WARN] [1584396242.510896212]: Controller  failed with error PATH_TOLERANCE_VIOLATED: 
[ WARN] [1584396242.511019370]: Controller handle  reports status ABORTED
[ INFO] [1584396242.511074851]: Completed trajectory execution with status ABORTED ...

Use Case and Setup

ROS melodic (installed from deb package) on Ubuntu 18.04

Universal Robot driver installed using these instructions

UrSim installed with external control UrCap

Project status at point of discovered

I observe this issue any time I try to execute a moveit plan on the UrSim UR3e

Steps to Reproduce

1. Execute the following launch file to launch the driver and moveit. Note: IP address will need to be modified to the machine running UrSim.

<launch>
  <arg name="robot_ip" default="XXX.XXX.XXX.XXX" />
  <arg name="debug" default="false" />

  <!-- Limited joint angles are used. Prevents complex robot configurations and makes the planner more efficient -->
  <arg name="limited" default="true" />

  <!-- Launch universal robot driver -->
  <include file="$(find ur_robot_driver)/launch/ur3e_bringup.launch">
    <arg name="robot_ip" default="$(arg robot_ip)"/>
    <arg name="limited" default="$(arg limited)"/>
    <arg name="debug" default="$(arg debug)" />
  </include>

  <!-- Remap follow_joint_trajectory from moveit to the UR driver -->
  <remap from="/follow_joint_trajectory" to="/scaled_pos_traj_controller/follow_joint_trajectory"/>

  <!-- Launch moveit -->
  <include file="$(find ur3_e_moveit_config)/launch/move_group.launch">
    <arg name="limited" default="$(arg limited)"/>
    <arg name="debug" default="$(arg debug)" />
  </include>

</launch>

2. Run the following Python script.

   
   #! /usr/bin/env python2

import sys import copy import math import rospy import tf import numpy as np import moveit_commander import moveit_msgs.msg

from geometry_msgs.msg import Pose, PoseStamped, Quaternion

group_name = "manipulator"

def get_pose(x, y, z, pitch, roll, yaw):

quaternion = tf.transformations.quaternion_from_euler(np.deg2rad(roll), np.deg2rad(pitch), np.deg2rad(yaw))

pose = Pose()
pose.orientation = Quaternion(*quaternion)
pose.position.x = x
pose.position.y = y  
pose.position.z = z 

return pose

Create scene objects for collision avoidance

def setup_scene():

scene = moveit_commander.PlanningSceneInterface()
robot = moveit_commander.RobotCommander()

# Need some time for things to initialize. 
# Without this the calls below do nothing.
# When using the C++ interface this is not required
rospy.sleep(2)

# Remove all objects created in previous executions
scene.remove_world_object()

# Create a table for the robot to sit on
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = 0.0
p.pose.position.y = 0.0
p.pose.position.z = -0.05
p.pose.orientation.w = 1.0
scene.add_box("table", p, (2.0, 2.0, 0.1))

def main():

moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python', anonymous=True)

move_group = moveit_commander.MoveGroupCommander(group_name)

display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
                                                moveit_msgs.msg.DisplayTrajectory,
                                                queue_size=20)

setup_scene()

radius_m = 0.5
height_m = 0.3
step_size_deg = 5.0

# Sweep angles from 0 to 2 PI in step size increments
angles = np.linspace(0, 2 * np.pi, 360 / step_size_deg)

roll = 0.0
pitch = 0.0
yaw = 0.0

# Rotate the root end effector in a circle
for angle in angles:

    x = radius_m * math.cos(angle)
    y = radius_m * math.sin(angle) 
    z = height_m

    pose = get_pose(x, y, z, pitch, roll, yaw)

    move_group.set_pose_target(pose)
    move_group.go(wait=True)
    move_group.stop()
    move_group.clear_pose_targets()

    yaw += step_size_deg

if name== "main": main()

[sharpe-developer]

The path tolerance is also violated when using the test_move.py script in fmauch_universal_robot/ur_driver. This script does not use moveit so it seems the issue is with the controller/action server interacting with ursim.

It is not obvious that the path tolerance is being violated when running the test_move.py script as it fails silently. Adding the following lines to the script will reveal a result value of -4 which is the value for PATH_TOLERANCE_VIOLATED

print("Result = " + str(client.get_result()))

[Karthik-Ranga]

Have you updated the action server in the controller file? It seems that the error is caused by the action server.

btw, do not use the test_move.py as it was for the older ur driver version.

[sharpe-developer]

@Karthik-Ranga How would I update the action server in the controller file?

The test_move.py script is using standard ROS action_lib interface. If the action server topic is changed to /scaled_pos_traj_controller/follow_joint_trajectory I think this script is valid for the new driver.

[Karthik-Ranga]

I am not sure whether that would work, but you can give it a try.

But for this driver to work, go to the respective robot's controller file in the moveit_config folder, under config folder you'll find the controllers.yaml file, you have to update the action server in that file.

[sharpe-developer]

@Karthik-Ranga Thank you for the response.

With the ur_robot_driver the instructions state to clone the repo for the Universal_Robots_ROS_Driver and the fmauch/universal_robot.

The Universal_Robots_ROS_Driver repo has a controller file located at Universal_Robots_ROS_Driver/ur_robot_driver/config/ur3_controllers.yaml. See the scaled_pos_traj_controller section for that file below

scaled_pos_traj_controller:
   type: position_controllers/ScaledJointTrajectoryController
   joints: *robot_joints
   constraints:
      goal_time: 0.6
      stopped_velocity_tolerance: 0.05
      shoulder_pan_joint: {trajectory: 0.2, goal: 0.1}
      shoulder_lift_joint: {trajectory: 0.2, goal: 0.1}
      elbow_joint: {trajectory: 0.2, goal: 0.1}
      wrist_1_joint: {trajectory: 0.2, goal: 0.1}
      wrist_2_joint: {trajectory: 0.2, goal: 0.1}
      wrist_3_joint: {trajectory: 0.2, goal: 0.1}
   stop_trajectory_duration: 0.5
   state_publish_rate: *loop_hz
   action_monitor_rate: 10

The fmauch repo has a controller located at /home/user/ur_ros_workspace/src/fmauch_universal_robot/ur3_e_moveit_config/config/controllers.yaml. This file just lists joint names. See file contents below

controller_list:
  - name: ""
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    joints:
      - shoulder_pan_joint
      - shoulder_lift_joint
      - elbow_joint
      - wrist_1_joint
      - wrist_2_joint
      - wrist_3_joint

Which file should be changed?

What would you change in the controller file?

I have tried modifying the trajectory: 0.2 in the first file but it seems the the path error increases and decreases with this value. In other words the current path tolerance will fail with values slightly larger than 0.2 (on average). But when I change the trajectory value to 0.3 then the path tolerances start failing with values above 0.3 (on average). When I change the trajectory value to 0.1 then the path tolerances start failing with values above 0.1 (on average). I'm very confused by this.

I'm also confused with the use of multiple repos (fmauch and Universal_Robots_ROS_Driver) as instructed in the Universal_Robots_ROS_Driver README. When using moveit files from the fmauch repo are used as that is where all the moveit config files are located but I am using the driver in the Universal_Robots_ROS_Driver repo. Is there something wrong with this approach?

[gavanderhoorn]

I'm also confused with the use of multiple repos (fmauch and Universal_Robots_ROS_Driver) as instructed in the Universal_Robots_ROS_Driver README. When using moveit files from the fmauch repo are used as that is where all the moveit config files are located but I am using the driver in the Universal_Robots_ROS_Driver repo.

The file in Universal_Robots_ROS_Driver/ur_robot_driver/config is for the driver. It contains important settings for the correct functioning of the driver itself and the ros_control controllers.

The file in fmauch_universal_robot/ur3_e_moveit_config/config is for MoveIt. It contains settings for MoveIt which tell it how to communicate with the driver.

While both filenames contain the word "controller", they don't really have anything much to do with each other.

[Karthik-Ranga]

You need both the repos to make the robot work. Controllers.yaml in fmauch repo is to be modified. This is the controller used to control the robot. In the line - name: "" replace with - name: /scaled_pos_traj_controller for you to be able to control the robot and use the tutorial in the below link to control the robot. http://docs.ros.org/melodic/api/moveit_tutorials/html/doc/move_group_python_interface/move_group_python_interface_tutorial.html

[sharpe-developer]

@Karthik-Ranga The link you provided is not working but I know the tutorial you are referring to. The code snippet in my original post was modeled after that tutorial.

Note: I can control the ursim simulated robot in moveit. The problem is the movement is very erratic and the controller keeps aborting. I was using a remap in my launch file to connect moveit to the scaled_pos_traj_controller.

I have replaced - name: "" replace with - name: /scaled_pos_traj_controller but I get the same error (see the log messages below ). I have recompiled the scaled_pos_traj_controller with verbose=true to get a little more info in the log messages.

[ERROR] [1584547171.254398827]: Path tolerances failed for joint: wrist_3_joint
[ERROR] [1584547171.254465896]: Path state tolerances failed:
[ERROR] [1584547171.254520822]: Position Error: 0.201685 Position Tolerance: 0.2
[ WARN] [1584547171.297165492]: Controller /scaled_pos_traj_controller failed with error PATH_TOLERANCE_VIOLATED: 
[ WARN] [1584547171.297349194]: Controller handle /scaled_pos_traj_controller reports status ABORTED
[ INFO] [1584547171.297447301]: Completed trajectory execution with status ABORTED ...

The position error (on average) always seems to track the trajectory setting in the Universal_Robots_ROS_Driver controller file. I'm not sure why.

[gavanderhoorn]

@sharpe-developer: do you have the real-time kernel setup and active? An e-Series controller will run at 500 Hz. I'm not sure whether the driver can keep up with that without a real-time kernel.

I'm also not sure how ursim will behave without one. Stable 500 Hz is not achievable with a regular Linux kernel in my experience.

The fact that "RViz and Gazebo work perfectly" means nothing. They don't use the same control infrastructure as the driver is not used at all there.

[sharpe-developer]

@gavanderhoorn I am using ursim simulated robot. There is no real robot connected. Just the simulated robot in polyscope ursim. The ursim software accepts one of four arguments (UR3, UR5, UR10, or UR16) to specify the robot type. There is no argument to indicate e-series so I assumed the ursim simulation was not aware if it was running an e-series on not.

I do not have a real-time kernel. I wondered if that could be an issue for the e-series so I tried using the UR3 (not the e-series) and the UR5 (not the e-series) configs on the ROS side. ursim does not distinguish between e-series or not so I simply specified UR3 or UR5 when starting ursim. However I get the same failure for all robot types (UR3e, UR3, and UR5)

[gavanderhoorn]

I am using ursim simulated robot. There is no real robot connected.

I am aware of this.

Just the simulated robot in polyscope ursim. The ursim software is started with one of three arguments (UR3, UR5, UR10) to specify the robot type. There is no argument to indicate e-series so I assumed the ursim simulation was not aware if it was running an e-series on not.

If the version of ursim > 5, you are running an e-series version iiuc.

I do not have a real-time kernel. I wondered if that could be an issue for the e-series so I tried using the UR3 (not the e-series) and the UR5 (not the e-series) configs on the ROS side.

That's not really how it works.

Control flow (and rate) is determined by the robot controller (or in your case: ursim). Configuring local controllers (ie: on the ROS side) to run at a lower rate is possible, but not necessarily a good idea.

ursim does not distinguish between e-series or not so I simply specified UR3 or UR5 when starting ursim. However I get the same failure for all robot types (UR3e, UR3, and UR5)

Again, afaik, any ursim version > 5 will essentially be simulating an "e-series" controller (or at least running the same version of polyscope and URControl).

Whether this means it's trying to achieve a control rate of 500 Hz is something you'd have to check. I have not verified.

---

And I also don't know whether this is the cause of the issue you are encountering.

[sharpe-developer]

Control flow (and rate) is determined by the robot controller (or in your case: ursim). Configuring local controllers (ie: on the ROS side) to run at a lower rate is possible, but not necessarily a good idea.

Agreed. I tried running the controller on the ROS side at 25 Hz (same as gazebo uses) and 125Hz (non e-series rate). All failed path tolerances.

Again, afaik, any ursim version > 5 will essentially be simulating an "e-series" controller (or at least running the same version of polyscope and URControl).

That is good to know. I was not aware of that. I am running the latest Polyscope version 5.7

I will try to setup a real-time kernel. I'm running Ubutnu and ROS in a virtual machine. Hopefully that is not an issue.

Do you know of anyone successfully controlling an e-series from ROS?

In your opinion, would I expect the same results when using a real robot vs the simulation?

[gavanderhoorn]

Agreed. I tried running the controller on the ROS side at 25 Hz (same as gazebo uses)

afaik, Gazebo runs ros_control at 1kHz internally. Where did you find the 25 Hz figure?

I'm running Ubutnu and ROS in a virtual machine. Hopefully that is not an issue.

Yes, that is going to be an issue. Installing a real-time kernel in a VM does not make much sense.

Do you know of anyone successfully controlling an e-series from ROS?

Plenty of people, actually. The driver is fully capable of working with e-Series controllers.

In your opinion, would I expect the same results when using a real robot vs the simulation?

I would not expect you to have these issues, at all, with a real robot.

At this point I suspect there is something going wrong in the interaction between ursim and the driver. The fact you're running things in a VM is not going to help, but I cannot tell you how that influences things at the moment.

---

Forget about installing a real-time kernel in your VM, as it's very likely not going to help.

Running the driver in a VM is not going to help either. If/when you start using a real robot, please do not attempt to do so with a VM.

[sharpe-developer]

afaik, Gazebo runs ros_control at 1kHz internally. Where did you find the 25 Hz figure?

In the controller file /fmauch_universal_robot/ur_e_gazebo/controller/arm_controller_ur3e.yaml the following setting is present state_publish_rate: 25

This value is set to 500 in the ur3e controller file and 125 in the ur3 controller file. I assumed that was setting the rate.

[sharpe-developer]

@gavanderhoorn

I would not expect you to have these issues, at all, with a real robot.

You are correct. I tried using the same setup with a real robot (UR3e). The real robot is controlled from the same non-real-time virtual machine that demonstrated the issues previously. If the simulation switch on the ursim teach pendant is enabled then the path tolerance error occurs. If the simulation switch is disabled (i.e. moving the physical robot) then no errors occur.

Seems that the issue must be in the simulation provided by the ursim software since I can control the physical robot using the identical setup without any issues.

I would expect the real robot to be less forgiving than the simulated robot (have stricter timing/rate requirements, etc.) so I was surprised it worked flawlessly when the simulation switch was disabled. I'm not familiar with the UrSim architecture. What is different when running with the teach pendant simulation switch enabled vs disabled that might cause the path tolerance error?

[gavanderhoorn]

afaik, Gazebo runs ros_control at 1kHz internally. Where did you find the 25 Hz figure?

In the controller file /fmauch_universal_robot/ur_e_gazebo/controller/arm_controller_ur3e.yaml the following setting is present state_publish_rate: 25

That's the actionlib status publication rate. It has nothing to do with the actual control rate.

This value is set to 500 in the ur3e controller file and 125 in the ur3 controller file. I assumed that was setting the rate.

These are different numbers. They have (almost) no relation to the state_publish_rate.

That's the same state_publish_rate. Personally I would not have set those equal to the main loop_hz. Perhaps @fmauch can explain why they are configured this way right now.

But even for these nrs: this is not linked to the control rate. It just happens to "reuse" the number (ie: 125 or 500) for a different setting. And the setting only influences how often the internal state of the action is published. Not the control rate.

The real robot is controlled from the same non-real-time virtual machine that demonstrated the issues previously.

While I'm happy for you things (appeared to) work, personally I would never run this driver from within a VM. You will not get the same performance, especially not when other nodes start competing for the same resources.

[sharpe-developer]

I agree with you. For our final production environment we will use a dedicated machine with a real-time patch. At this early stage in our development with the UR3e we simply wanted an environment that we could write code and test movements in a simulated environment. I wanted to use the UrSim simulation as it is the closest match to the actual robot and would allow a near seamless transition between simulation and real robot. But as stated previously this is not working out.

One major issue I can't understand. Is this an ROS VM/driver issue or is this issue in the Polyscope simulator. Based on earlier comments the assumption is an issue with the ROS VM and driver. I'm having trouble understanding how this is still true given that the same VM controls the real robot without violating path tolerance. Since the only thing that is changing is the use of the simulator vs the real robot (all of which are running on the UR control box and teach pendant) do you still think the ROS VM or ROS driver is the problem?

[sharpe-developer]

It seems that the simulation switch in Polyscope is not necessary when Polyscope is not running on the actual robot controller computer. If I run Polyscope on a standalone laptop I still see the robot move on the Graphics tab when the simulation switch is off. The only time simulation would be required is when communicating with Polysope on the actual robot control computer (so the real robot arm doesn't move).

[gavanderhoorn]

It seems that the simulation switch in Polyscope is not necessary when Polyscope is not running on the actual robot controller computer. If I run Polyscope on a standalone laptop I still see the robot move on the Graphics tab when the simulation switch is off. The only time simulation would be required is when communicating with Polysope on the actual robot control computer (so the real robot arm doesn't move).

Yes. Correct.

I assumed that was common knowledge.

Running ursim already simulates everything, so there is no need to switch to the "simulated robot".

[sharpe-developer]

I assumed that was common knowledge.

I did not know this. I assumed simulation = simulation and not simulation = real robot without simulation.

From the Polyscope manual:

To the left of the Dashboard the Simulation and Real Robot buttons toggle between running the program in a simulation, or running it on the real robot. When running in simulation, the robot arm does not move and thus cannot damage itself or any nearby equipment in collisions. Use simulation to test programs if unsure about what the robot arm will do.

This issue is only applicable to simulation mode. Everything works great when simulation is disabled. The disadvantage is I have to have 2 installations of Polyscope in order to use ursim for "simulation" without moving the real robot. But for the time being I can workaround this issue by installing one instance of Polyscope on a "simulation computer" and another instance on the real robot.

[gavanderhoorn]

The disadvantage is I have to have 2 installations of Polyscope in order to use ursim for "simulation" without moving the real robot. But for the time being I can workaround this issue by installing one instance of Polyscope on a "simulation computer" and another instance on the real robot.

To be clear: you have one installation of ursim and you have a real robot (or multiple real robots).

I would not call this "two installations of Polyscope" and then make the distinction about whether you run them on your PC or on the controller. That just confuses everyone.

on a "simulation computer"

This could just be your regular development PC. No need to use a separate machine for it.

You could even run things in a Docker container if that would help.

[sharpe-developer]

You are right, I am confused. I'm fairly confident that I do have Polyscope installed in two places. That's what I meant by two installations. We have a single robot arm which has a industrial control computer and a teach pendant attached. This is one instance/installation of Polyscope. I cannot use that instance/installation for development because I cannot use simulation mode and hence (to my understanding) the real robot will move. Because of the simulation switch issue I have to install the Polyscope software somewhere else so I can run tests and the robot will not move. Yes, it could be on my development machine (which it is) or a Docker container or any other computer besides the one the robot is connected to but this is still a second installation of the software is it not?

Either way I think the issue is well documented and I have a workaround. It would be nice if simulation mode worked but it is not blocking my development. Thank you for all your time and assistance!

[fmauch]

Jut to add this to the discussion:

As soon as a simulation or a virtual system is included into the system, real-time requirements will not be met and therefore any path tolerances are expected to be missed potentially. It might perfectly fine in many situations, but that's not the thing that should be expected.

If you only want to test your code and whether the robot is doing the right thing, you can raise the path tolerances inside the controller file during development. Be aware that this will allow the robot to deviate from its path for a larger extent, so make sure not to use this inside a production environment.

I agree with @gavanderhoorn that calling a URsim and a real robot "two installations of Polyscope" is kind of misleading. The main difference is the actual controller which is a real-time machine on the real robot and a non-real-time simulation in the case of URSim. What the exact result of the "Simulation" switch on the TP is I honestly don't know. If you want to simulate a robot for development, use URSim (without the "Simulation" switch).

@gavanderhoorn I think the state_publish_rate is the same as the control loop rate only for legacy reasons as this was the same in the ur_modern_driver for CBx series and every 125 Hz was replaced by 500Hz and later by *loop_hz without putting too much thought into that...

@sharpe-developer I'll go ahead and close this as I interpreted your last post that this would be fine with you.