Pb defining cinematic for FANUC M710ic/70 robot from urdf

[sancelot]

Hi,
I would like being able to simulate a fanuc m710ic/70 robot (same mechanics as m710ic/50)

I first was starting from ROS urdf file, but I have problems with the robot origin.

robot origin in Fanuc sw is defined on the red point :

When I am loading the urdf file, and check home position, this origin (z axis translation)is not taken into account .
Here is the output of each joint:

it should be :

I have seen in the urdf file there is a joint named "base_link-base" for "Fanuc World Coordinates transform", but I don't really understand how it can be taken into account

the urdf I am using is is : ```

<!-- joints: main serial chain -->
<joint name="joint_1" type="revolute">
  <origin xyz="0 0 0.565" rpy="0 0 0" />
  <parent link="base_link" />
  <child link="link_1" />
  <axis xyz="0 0 1" />
  <limit effort="0" lower="-3.1415" upper="3.1415" velocity="3.0543" />
</joint>
<joint name="joint_2" type="revolute">
  <origin xyz="0.150 0 0" rpy="0 0 0"/>
  <parent link="link_1" />
  <child link="link_2" />
  <axis xyz="0 1 0" />
  <limit effort="0" lower="-1.5707" upper="2.3561" velocity="3.0543" />
</joint>
<joint name="joint_3" type="revolute">
  <origin xyz="0 0 0.870" rpy="0 0 0"/>
  <parent link="link_2" />
  <child link="link_3" />
  <axis xyz="0 -1 0" />
  <limit effort="0" lower="-2.7925" upper="4.8869" velocity="3.0543" />
</joint>
<joint name="joint_4" type="revolute">
  <origin xyz="0 0 0.170" rpy="0 0 0"/>
  <parent link="link_3"/>
  <child link="link_4"/>
  <axis xyz="-1 0 0"/>
  <limit effort="0" lower="-6.2830" upper="6.2830" velocity="4.3633" />
</joint>
<joint name="joint_5" type="revolute">
  <origin xyz="1.016 0 0" rpy="0 0 0"/>
  <parent link="link_4"/>
  <child link="link_5"/>
  <axis xyz="0 -1 0"/>
  <limit effort="0" lower="-2.1816" upper="2.1816" velocity="4.3633" />
</joint>
<joint name="joint_6" type="revolute">
  <origin xyz="0.175 0 0" rpy="0 0 0"/>
  <parent link="link_5"/>
  <child link="link_6"/>
  <axis xyz="-1 0 0"/>
  <limit effort="0" lower="-6.2831" upper="6.2831" velocity="6.1959" />
</joint>

<!-- ROS-Industrial 'base' frame: base_link to Fanuc World Coordinates transform -->
<link name="base" />
<joint name="base_link-base" type="fixed">
  <origin xyz="0 0 0.565" rpy="0 0 0"/>
  <parent link="base_link"/>
  <child link="base"/>
</joint>

<!-- ROS-Industrial 'flange' frame: attachment point for EEF models -->
<link name="flange" />
<joint name="joint_6-flange" type="fixed">
  <origin xyz="0 0 0" rpy="0 0 0" />
  <parent link="link_6" />
  <child link="flange" />
</joint>

<!-- ROS-Industrial 'tool0' frame: all-zeros tool frame -->
<link name="tool0" />
<joint name="link_6-tool0" type="fixed">
  <origin xyz="0 0 0" rpy="3.14159 -1.570795 0" />
  <parent link="flange" />
  <child link="tool0" />
</joint>

[stephane-caron]

Can you share your complete URDF file?

If it was generated from the Xacro in https://github.com/ros-industrial/fanuc/tree/melodic-devel/fanuc_m710ic_support you should be able to share it (BSD-3-Clause license).

[sancelot]

m710ic70.zip

[stephane-caron]

OK, we can look at the frames of this robot description using e.g. the following script:

$ git clone https://github.com/robot-descriptions/robot_descriptions.py
$ cd robot_descriptions.py
$ python examples/display_urdf_frames.py fanuc_m710ic_description

It seems (to be double checked) that the red dot you mentioned corresponds to the link_1 frame. So, if I understand correctly, what you are looking for is the transform from the link_6 frame to the link_1 frame. In Pinocchio we can compute it as follows:

import pinocchio as pin
from robot_descriptions.loaders.pinocchio import load_robot_description

robot = load_robot_description("fanuc_m710ic_description")
pin.forwardKinematics(robot.model, robot.data, pin.neutral(robot.model))
transform_link_6_to_world = robot.data.oMi[6]
transform_link_1_to_world = robot.data.oMi[1]
transform_link_6_to_link_1 = transform_link_1_to_world.actInv(
    transform_link_6_to_world
)

print(f"Translation in mm: {1000. * transform_link_6_to_link_1.translation}")

Running this example yields:

Translation in mm: [1341.    0. 1040.]

(I ran pip install . from the cloned robot_descriptions.py as the description was just added and not released to PyPI yet.)

Hoping this helps!

[sancelot]

This not really what I expect to do, altough the result is right....

I need validating the fanuc urdf file to conform with fanuc software.
I want to validate frame pos = f(joint position) for each joints.

I am setting joint position and checks computed matrix for each joint , and tool position.
I am using this piece of c++ code :

 Model model;
  pinocchio::urdf::buildModel(urdf_filename,model);
  std::cout << "model name: " << model.name << std::endl;
  
  // Create data required by the algorithms
  Data data(model);
  
  // Sample a random configuration
//  Eigen::VectorXd q = randomConfiguration(model);
  Eigen::VectorXd q(6);
  // robot joints 
    q <<   0. , 10. * 3.14159/180.,0 , 0 , 0, 0;
    std::cout << "q: " << q.transpose() << std::endl;
    // Perform the forward kinematics over the kinematic tree
    forwardKinematics(model, data, q);

    // cartesian pose
    // Print out the placement of each joint of the kinematic tree
    for (JointIndex joint_id = 0; joint_id < (JointIndex)model.njoints; ++joint_id)
        std::cout << std::setw(24) << std::left
                  << model.names[joint_id] << ": "
                  << std::fixed << std::setprecision(4)
                  << data.oMi[joint_id].translation().transpose() * 1000
                  << std::endl;

I think there is 2 entry points in the ros fanuc urdf file , Depending on which one is used, you will use ros conventions may be or fanuc convention, I modified the urdf file a bit - I am not sure the right way - to add the frame offset and give me the right result for home pos:

[stephane-caron]

OK, sounds good. Let's convert this topic to a discussion so we don't have to close it (issues are rather for things to fix and closed when done).

[sancelot]

Right. With the small offset modification in urdf file, I managed to validate the urdf file conforms to fanuc sw positions for m710ic70 robot.
NB : Be careful with J2/J3 interaction.