Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Mismatch for axis 3 in R-1000iA #17

Closed
VictorLamoine opened this issue May 2, 2016 · 17 comments
Closed

Mismatch for axis 3 in R-1000iA #17

VictorLamoine opened this issue May 2, 2016 · 17 comments

Comments

@VictorLamoine
Copy link
Contributor

On the teach pendant:

J3: -33.342 (deg)

Using robot_state_visualize (indigo-devel branch):

$ rostopic echo /joint_states
header: 
  seq: 4802
  stamp: 
    secs: 1462191929
    nsecs: 471022367
  frame_id: ''
name: ['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6']
position: [0.2743533253669739, 0.5467115640640259, -0.035214900970458984, 1.2852824926376343, -1.7649667263031006, -0.5436538457870483]
velocity: []
effort: []

-0.035214900970458984 rad = -2.017 deg
@gavanderhoorn
Copy link
Member

With such a difference I would expect the robot in RViz to be in a radically different pose compared to the real robot. Have you observed that as well?

@VictorLamoine
Copy link
Contributor Author

VictorLamoine commented May 2, 2016 •
edited

No the robot is at the right place!

capture du 2016-05-02 14 34 14

dsc_0003

@gavanderhoorn
Copy link
Member

gavanderhoorn commented May 2, 2016 •
edited

This is caused by the J2-J3 linkage Fanucs have. If I do J3 - J2 for your values, I get -0.035.. - 0.546.. = -0.581... In degrees, that is -33.342...

@gavanderhoorn
Copy link
Member

Some controllers / software versions allow you to enable a display item that shows you the J2-J3 linkage value (or something, can't remember what it is called exactly). That should show you the difference as well.

@gavanderhoorn
Copy link
Member

No the robot is at the right place!

good :) I tend to test these things quite rigorously, but it's always possible I made a mistake, obviously.

A rosrun tf tf_echo base tool0 is also a good test. That should correspond to the TCP's coordinates in the World frame.

@VictorLamoine
Copy link
Contributor Author

Yes I have J2/J3 interaction showing the right value (the one displayed in rostopic echo)

@gavanderhoorn
Copy link
Member

Closing then as a works-for-me.

@VictorLamoine
Copy link
Contributor Author

VictorLamoine commented May 2, 2016 •
edited

It works fine with rosrun tf tf_echo base tool0, I get the right pose.

I realized that when using the caljob_creator which records (false in my case) joint values to create calibration poses. copying joint values from rostopic echo to the caljob.yaml file.
When executing the recorded poses, the robot doesn't go at the expected pose!

@gavanderhoorn
Copy link
Member

gavanderhoorn commented May 2, 2016 •
edited

I realized that when using the caljob_creator which records (false) joint values to create calibration poses.
When executing the recorded poses, the robot doesn't go at the expected pose!

How are you 'executing the recorded poses'? Trajectories going through the motion_streaming_interface in fanuc_driver should have their JointTrajectoryPoints updated so as to take the J2-J3 coupling into account. In all cases the robot should then go to the correct pose (as long as the J23_factor parameter is set correctly).

@VictorLamoine
Copy link
Contributor Author

The node parsing the YAML file is giving that to an other node executing the trajectory for me 😄
I think it's https://github.com/ros-industrial/industrial_calibration/blob/hydro-devel/industrial_extrinsic_cal/action/robot_joint_values_trigger.action

I just made the small calculation (addition) to the joint values and everything works well now 👍

@gavanderhoorn
Copy link
Member

I just made the small calculation (addition) to the joint values and everything works well now 👍

I'm not entirely sure how the caljob_creator works, but it should never be necessary to compensate for the J2-J3 coupling outside the nodes in fanuc_driver. If you find you need to, then something is wrong and I'd like to fix it.

@VictorLamoine
Copy link
Contributor Author

VictorLamoine commented May 2, 2016 •
edited

caljob_creator creates a YAML file that store poses for the extrinsic calibration.
You run this node, move your robot to poses that looks good for the calibration and record these poses.

At the end you get a .YAML file that looks like that:

---
reference_frame: base
scenes:
-
     scene_id: 0
     trigger: ROS_ROBOT_JOINT_VALUES_ACTION_TRIGGER
     trig_action_server: rosRobotSceneTrigger_joint_values
     joint_values:
        - 0.27438
        - 0.54669
        - -0.03522
        - 1.28528
        - -1.76496
        - -0.54365
     observations:
     -
          camera: sls_2
          target: CircleGrid10x10
          roi_x_min: 0
          roi_x_max: 1280
          roi_y_min: 0
          roi_y_max: 960
          cost_type: LinkCameraCircleTargetReprjErrorPK
-
     scene_id: 1
     trigger: ROS_ROBOT_JOINT_VALUES_ACTION_TRIGGER
     trig_action_server: rosRobotSceneTrigger_joint_values
     joint_values:
        - 0.39575
        - 0.58015
        - -0.05061
        - 1.44278
        - -1.90017
        - -0.61905
     observations:
     -
          camera: sls_2
          target: CircleGrid10x10
          roi_x_min: 0
          roi_x_max: 1280
          roi_y_min: 0
          roi_y_max: 960
          cost_type: LinkCameraCircleTargetReprjErrorPK
-
     scene_id: 2
     trigger: ROS_ROBOT_JOINT_VALUES_ACTION_TRIGGER
     trig_action_server: rosRobotSceneTrigger_joint_values
     joint_values:
        - 0.26674
        - 0.41834
        - 0.41834
        - 1.29968
        - -1.67077
        - -0.61933
     observations:
     -
          camera: sls_2
          target: CircleGrid10x10
          roi_x_min: 0
          roi_x_max: 1280
          roi_y_min: 0
          roi_y_max: 960
          cost_type: LinkCameraCircleTargetReprjErrorPK
-
     scene_id: 3
     trigger: ROS_ROBOT_JOINT_VALUES_ACTION_TRIGGER
     trig_action_server: rosRobotSceneTrigger_joint_values
     joint_values:
        - 0.27491
        - 0.30231
        - -0.30266
        - 0.94138
        - -2.06026
        - -0.95086
     observations:
     -
          camera: sls_2
          target: CircleGrid10x10
          roi_x_min: 0
          roi_x_max: 1280
          roi_y_min: 0
          roi_y_max: 960
          cost_type: LinkCameraCircleTargetReprjErrorPK
-
     scene_id: 4
     trigger: ROS_ROBOT_JOINT_VALUES_ACTION_TRIGGER
     trig_action_server: rosRobotSceneTrigger_joint_values
     joint_values:
        - 0.04786
        - 0.16993
        - -0.61908
        - 0.74808
        - -1.47835
        - -1.03552
     observations:
     -
          camera: sls_2
          target: CircleGrid10x10
          roi_x_min: 0
          roi_x_max: 1280
          roi_y_min: 0
          roi_y_max: 960
          cost_type: LinkCameraCircleTargetReprjErrorPK
-
     scene_id: 5
     trigger: ROS_ROBOT_JOINT_VALUES_ACTION_TRIGGER
     trig_action_server: rosRobotSceneTrigger_joint_values
     joint_values:
        - 0.14930
        - 0.53925
        - -0.02988
        - 1.26840
        - -1.18279
        - -0.38326
     observations:
     -
          camera: sls_2
          target: CircleGrid10x10
          roi_x_min: 0
          roi_x_max: 1280
          roi_y_min: 0
          roi_y_max: 960
          cost_type: LinkCameraCircleTargetReprjErrorPK
-
     scene_id: 6
     trigger: ROS_ROBOT_JOINT_VALUES_ACTION_TRIGGER
     trig_action_server: rosRobotSceneTrigger_joint_values
     joint_values:
        - 0.31690
        - 0.58545
        - -0.02899
        - 1.75386
        - -0.49604
        - -0.38491
     observations:
     -
          camera: sls_2
          target: CircleGrid10x10
          roi_x_min: 0
          roi_x_max: 1280
          roi_y_min: 0
          roi_y_max: 960
          cost_type: LinkCameraCircleTargetReprjErrorPK
-
     scene_id: 7
     trigger: ROS_ROBOT_JOINT_VALUES_ACTION_TRIGGER
     trig_action_server: rosRobotSceneTrigger_joint_values
     joint_values:
        - 0.42605
        - 0.15085
        - -0.55842
        - 2.41646
        - -0.87343
        - -1.02646
     observations:
     -
          camera: sls_2
          target: CircleGrid10x10
          roi_x_min: 0
          roi_x_max: 1280
          roi_y_min: 0
          roi_y_max: 960
          cost_type: LinkCameraCircleTargetReprjErrorPK
-
     scene_id: 8
     trigger: ROS_ROBOT_JOINT_VALUES_ACTION_TRIGGER
     trig_action_server: rosRobotSceneTrigger_joint_values
     joint_values:
        - 0.31533
        - 0.22557
        - -0.4334
        - 3.10028
        - -0.47122
        - -1.61239
     observations:
     -
          camera: sls_2
          target: CircleGrid10x10
          roi_x_min: 0
          roi_x_max: 1280
          roi_y_min: 0
          roi_y_max: 960
          cost_type: LinkCameraCircleTargetReprjErrorPK

optimization_parameters: xx

I wanted to edit some poses because I was too close to the target.
So I moved the robot manually to the pose number 3 and, got away from the target and copied the values I got from rostopic echo /joint_states in the YAML file, unfortunately doing this leads to the robot going to another pose that the one you wanted to go.

I don't know if that means something's wrong, tell me if my explanation is not clear enough!

@gavanderhoorn
Copy link
Member

Ah now I understand you.

I think this is unfortunately something that isn't really 'fixable': the Fanuc TP will show you the angle J3 is making including the J2/J3 interaction. ROS however will always want the angle J3 is making wrt J2. This cannot include the J2/J3 interaction, as then it's essentially the angle J3 makes wrt the base frame, not wrt J2. So the driver removes the contribution due to the interaction when broadcasting joint states, and adds it when sending JointTrajectoryPoints to the controller.

As you found out, converting joint angles you get from the TP into radians then doesn't work, or at least not without taking the J2/J3 interaction into account.

I just made the small calculation (addition) to the joint values and everything works well now 👍

I may have misunderstood your comment earlier: I thought you compensated for J2-J3 in your code. I now believe you did the math and then adjusted the values in the joint_values array in the yaml. Is that correct? If so, then that is acceptable / the correct approach.

@VictorLamoine
Copy link
Contributor Author

Ok then I'll do with it, it's no big deal, the big red button on the teach is not useless.. 😉

Yes I just fixed the joint_values yaml array by doing the math.
Glad to hear I'm not doing things crazy :)

@gavanderhoorn
Copy link
Member

I think I'll add an entry to the Troubleshooting page about this.

Thanks for bringing this (ie: the non-obviousness of it) to my attention.

@gavanderhoorn
Copy link
Member

Related ROS Answers question btw: Strange behaviour from joint_trajectory_action to ABB robot.

@gavanderhoorn gavanderhoorn mentioned this issue May 2, 2016
Closed
@gavanderhoorn
Copy link
Member

@VictorLamoine: bit late, but I've added an entry to the FAQ about J2/J3 interaction. See ros-industrial/fanuc#186.

@gavanderhoorn gavanderhoorn mentioned this issue Jun 4, 2020
Closed
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Assignees
No one assigned
Labels
None yet
Milestone
No milestone
Development

No branches or pull requests
2 participants
@gavanderhoorn @VictorLamoine