odrya
October 27, 2021, 8:05am
1
Dear UR Community!
I would like to control our UR5 robot with the official ROS driver.
The setup and building steps are executed without any problems. I was able to succesfully communicate with the robot over the network. However, after the ur_calibration
step the robot pose shown in RViZ is totally different than the robot pose in reallity.
I will try to summarize everything as follows.
The robot pose in the reallity is shown below:
Dropbox - IMG_20211027_090903_resize2.jpg - Simplify your life
Dropbox - IMG_20211027_091814_resize2.jpg - Simplify your life
After launching
roslaunch ur_calibration calibration_correction.launch \
robot_ip:=192.168.1.81 \
target_filename:="$(rospack find example_organization_ur_launch)/etc/ex-ur5-1_calibration.yaml"
I get the following yaml file:
kinematics:
shoulder:
x: 0
y: 0
z: 0.08948760745641905
roll: -0
pitch: 0
yaw: 5.724267271354014e-05
upper_arm:
x: 4.767387894761116e-05
y: 0
z: 0
roll: 1.569913581252249
pitch: 0
yaw: 2.121784526601501e-05
forearm:
x: -0.4254051262210206
y: 0
z: 0
roll: 3.141571256638476
pitch: 3.139609714587363
yaw: 9.181814845231772e-05
wrist_1:
x: -0.392460380822819
y: -0.002312531082237207
z: 0.1097805113913582
roll: 0.02106192704769751
pitch: -0.001508302033929348
yaw: 3.141509032753284
wrist_2:
x: 7.021751572921595e-05
y: -0.09475180859812876
z: -0.0003085072123167084
roll: 1.574052265934258
pitch: 0
yaw: -3.378771318114996e-05
wrist_3:
x: -4.120584958757715e-05
y: 0.0824868794107365
z: -0.0002106126115580783
roll: 1.568243046135182
pitch: 3.141592653589793
yaw: -3.141583455241135
hash: calib_2261225183801150179
Then, I bringup the ur5 with it’s launch file and the aforementioned kinematics yaml file. The RVIZ shows a totally different robot pose, however the joint angles are the same as on the TP. See the image below. (I’m a new user, and i cannot post more links)
I even checked the tool pose with a transform listener, and i got again different values than in the TP. See the image below.
(I’m a new user, and i cannot post more links)
Can you give hints where can be the problem?
Thank you in advance.
Best regards.
odrya
October 27, 2021, 8:07am
2
mauch
December 1, 2021, 1:36pm
3
Have you checked one of the following issues?
opened 11:10PM - 16 Sep 21 UTC
closed 04:36PM - 21 Sep 21 UTC
# Summary
There seems to be a discrepancy of approximately 400mm in the Z axi… s between the forward kinematics that appear in the teach pendant "Move" display, and the forward kinematics reported to ROS by the driver when requesting the transform between `'base'` and `'tool0_controller'`
# Versions
- ROS Driver version: ROS Noetic, driver commit 54e24c03f07e1a3c6c92569dcb24e51cc013b976
- Affected Robot Software Version(s): URSoftware 3.15.1.106171 (Mar 29 2021) ;
- Affected Robot Hardware Version(s): UR10
- Robot Serial Number: 2018300027
- UR+ product(s) installed:
- URCaps Software version(s): external control
# Impact
Ideally, we would like 100% consistency between the ROS world and what is seen on the teach pendant. It is reasonably important that a world-frame XYZ position reported on the pendant be meaningful when doing planning, etc. in ROS.
# Issue details
## Project status at point of discovered
Discovered this when writing some joystick teleop code for UR10. I wanted to make sure that the FK was being computed correctly by my ROS code and could not get my FK to match the reported robot coordinates.
## Steps to Reproduce
On the ROS side, obtained a robot calibration [per the instructions](https://github.com/UniversalRobots/Universal_Robots_ROS_Driver#extract-calibration-information). Then ran the bringup for UR10:
roslaunch ur_robot_driver ur10_bringup.launch robot_ip:=10.44.3.55 kinematics_config:=/path/to/ur10_calibration.yaml
Look at the pendant and see the following:
* Installation -> TCP Configuration: All zeros except for Z = 250.0 mm
* Move -> Joints: -90, -110, -130, 60, 90, 0; X: -164.86 mm, Y: -836.14 mm, Z: -77.33 mm
Then echo the transform from `base` to `tool0_controller`:
rosrun tf tf_echo base tool0_controller
The output contains`Translation: [-0.165, -0.836, 0.323]`.
X & Y agree with the teach pendant. Z is off by ~400mm.
I can similarly reproduce this in the simulator as well, although FK values are slightly different than real robot for the same joint configuration (possibly due to calibration offsets?).
Simulated teach pendant FK: [-174.15 mm, -861.88 mm, -18.68mm], ROS Translation: [-0.174, -0.862, 0.381].
## Expected Behavior
ROS TF output matches reported XYZ on Move screen in teach pendant or simulation window.
## Actual Behavior
Seemingly persistent offset of ~400 mm in Z between ROS transforms and Move screen.
opened 08:12AM - 06 May 20 UTC
closed 08:42AM - 06 May 20 UTC
Is there some kind of documentation on which TF frames are published by this dri… ver? As suggested in https://github.com/UniversalRobots/Universal_Robots_ROS_Driver/issues/127#issuecomment-605032696 I want to retrieve the TCP by making a TF lookup. I guess it hast to be one of the following transformations:
* world -> tool0_controller
* world -> tool0
* world -> ee_link
One thing I´m wondering as well is that none of this 3 transformations correspond to the TCP shown on the teachpendent.
I´m using an UR5e with Polyscope 5.6.0.
Every help is much appreciated.
tool0_controller
comes from the robot_controller and tool0
from the forward kinematics. If your calibration is correct, those two should align.
It might be interesting where the intermediate TFs are actually located in RViz