urdf joint config doesn't make sense

Hi, I am trying to get the gripper_grasp_frame pose in gazebo. However, gazebo will automatically merge fixed links to be a whole object, which makes gripper grasp link not accessible in gazebo model. The pose I can get is the arm_7_link, so I tried to get the grasp link pose by manually transform the pose from arm 7 link to grasp link. To do this, I need to get the D-H parameter for this transformation. I looked into your urdf file and found the following setting for one of the joints:

tiago_robot/tiago_description/urdf/gripper/gripper.urdf.xacro

Line 123 in 2892fb7

However, I don't think the setting showed above is correct, given that the relative pose the gripper_link and the grasp link is visualized as follows:

Is it a mistake or I miss something there? By the way, is there any easier way to get information I need?
Thanks!

tiago_robot/tiago_description/robots/tiago_steel.urdf.xacro

Lines 38 to 41 in 2892fb7

    
           <xacro:tiago_arm name="arm" parent="torso_lift_link" wrist_6_range="90.0"/> 
        
           <xacro:pal_gripper parent="arm_tool_link" name="gripper"> 
        
             <origin xyz="0.01 0 0" rpy="${90*deg_to_rad} ${90*deg_to_rad} ${-90*deg_to_rad}"/> 
        
           </xacro:pal_gripper>

The parent of the gripper is arm_tool_link:

tiago_robot/tiago_description/urdf/arm/arm.urdf.xacro

Lines 261 to 265 in 2892fb7

    
           <joint name="${name}_tool_joint" type="fixed"> 
        
             <parent link="${name}_7_link" /> 
        
             <child link="${name}_tool_link" /> 
        
             <origin xyz="0 0 0.046" rpy="${90.0 * deg_to_rad} ${-90.0 * deg_to_rad} ${180 * deg_to_rad}" /> 
        
           </joint>

Which is arm_7_link with a rotation applied and a bit of Z displacement. The rotation effectively makes the X axis look "going out" of the arm. So if you apply that 0.12 on X, well, that's where the grasping frame is. Checking the Rviz TF tree is maybe easier to see the actual tree and it's transformations. In the TF widget you can check the transforms in relation to the previous frames and so.

Good luck.

@awesomebytes Hi, thanks for your replay. Maybe I didn't make it clear. the joint setting from arm_7_link to arm_tool_link is fine, as you posted above. Where I am confused is the joint setting between gripper_link and gripper_grasp_frame. As the picture shows, it should be xyz=0 0 -0.12 or something.

So the gripper_grasping_frame has as parent gripper_link:

tiago_robot/tiago_description/urdf/gripper/gripper.urdf.xacro

Lines 120 to 124 in 2892fb7

    
           <joint name="${name}_grasping_frame_joint" type="fixed"> 
        
             <parent link="${name}_link" /> 
        
             <child link="${name}_grasping_frame" /> 
        
             <origin xyz="0.12 0 0" rpy="0 0 0" /> 
        
           </joint>

gripper_link has as parent arm_tool_link and has an applied the origin block:

tiago_robot/tiago_description/urdf/gripper/gripper.urdf.xacro

Lines 46 to 51 in 2892fb7

    
           <joint name="${name}_joint" type="fixed"> 
        
             <insert_block name="origin"/> 
        
             <parent link="${parent}" /> 
        
             <child link="${name}_link" /> 
        
             <axis xyz="0 0 0" /> 
        
           </joint>

Checking the main URDF:

tiago_robot/tiago_description/robots/tiago_steel.urdf.xacro

Lines 39 to 41 in 2892fb7

    
           <xacro:pal_gripper parent="arm_tool_link" name="gripper"> 
        
             <origin xyz="0.01 0 0" rpy="${90*deg_to_rad} ${90*deg_to_rad} ${-90*deg_to_rad}"/> 
        
           </xacro:pal_gripper>

Yeah, it makes no sense.

Dumping the URDF from the parameter server (after it has been passed thru xacro):

  <!-- Grasping frame -->
  <joint name="gripper_grasping_frame_joint" type="fixed">
    <parent link="gripper_link"/>
    <child link="gripper_grasping_frame"/>
    <origin rpy="-1.5708 1.5708 0" xyz="0 0 -0.12"/>
  </joint>

Looks like the rotation has been computed internally... and the XYZ has changed on it.

I double checked using xacro manually:

roscd tiago_description/robots
rosrun xacro xacro tiago_steel.urdf.xacro > steel.urdf

And I get the same result.

Also Rviz, obviously, shows it as the generated URDF.

Hmm I can only guess it's something we are missing on interpreting how the URDF/xacro works, or a bug in URDF/xacro @rhaschke do you have any idea about this? (I'm voting on us not understanding something, tho).

This is quite amusing.

from tf.transformations import quaternion_from_euler, euler_from_quaternion
import math
quat = quaternion_from_euler(math.radians(90), math.radians(90), math.radians(-90))
# array([  7.07106781e-01,   5.55111512e-17,  -7.07106781e-01, 1.66533454e-16])
roll, pitch, yaw = euler_from_quaternion(quat)
# (3.141592653589793, 1.5707963267948966, 0.0)

That rotation doesn't even match with the one we are getting. Going one step back, (from the screenshot) the rotation of gripper_link to arm_tool_link is:

euler_from_quaternion([0.70711, 1.7312e-12, -0.70711, 5.1936e-12])
# (1.5707963267961207, 1.57079632679, -1.5707963267948966)

Confirmed in the generated URDF too:

  <joint name="gripper_joint" type="fixed">
    <origin rpy="1.57079632679 1.57079632679 -1.57079632679" xyz="0.01 0 0"/>
    <parent link="arm_tool_link"/>
    <child link="gripper_link"/>
    <axis xyz="0 0 0"/>
  </joint>

Which, hey, is what we expected, 90deg, 90deg, -90deg.

Who is rotating and translating the gripper_grasping_frame?

Found it, we were looking in the wrong place, it's not using that gripper URDF, it's using this one:
https://github.com/pal-robotics/pal_gripper/blob/5ee011ffa60612da35432998eb72fa97e3e10071/pal_gripper_description/urdf/gripper.urdf.xacro#L145-L150

  <joint name="${name}_grasping_frame_joint" type="fixed">
    <parent link="${name}_link" />
    <child link="${name}_grasping_frame" />
    <origin xyz="0 0 -0.12" rpy="-1.5708 1.5708 0" />
  </joint>
  <link name="${name}_grasping_frame">

from the pal_gripper_description.

Yeah, you are right. I just overlooked this detail. It spawns the robot by launch gripper urdf from pal package, which is declared here:

tiago_robot/tiago_description/robots/tiago_steel.urdf.xacro

Lines 26 to 27 in 2892fb7

    
           <!-- Gripper --> 
        
           <xacro:include filename="$(find pal_gripper_description)/urdf/gripper.urdf.xacro" />

Thanks for your feedback and sorry for the trouble brought to you.

@huiwenzhang Best of luck!

You could submit a PR suggesting to remove the superfluous urdf

…

On Thu, 7 Jun 2018, 09:39 Sam Pfeiffer, ***@***.***> wrote: @huiwenzhang <https://github.com/huiwenzhang> Best of luck! — You are receiving this because you are subscribed to this thread. Reply to this email directly, view it on GitHub <#3 (comment)>, or mute the thread <https://github.com/notifications/unsubscribe-auth/ADXH4aOAwoToxk7T4iQ2pG7uFc_cmzoQks5t6ObNgaJpZM4UdsIb> .

	<xacro:tiago_arm name="arm" parent="torso_lift_link" wrist_6_range="90.0"/>
	<xacro:pal_gripper parent="arm_tool_link" name="gripper">
	<origin xyz="0.01 0 0" rpy="${90deg_to_rad} ${90deg_to_rad} ${-90*deg_to_rad}"/>
	</xacro:pal_gripper>

	<joint name="${name}_tool_joint" type="fixed">
	<parent link="${name}_7_link" />
	<child link="${name}_tool_link" />
	<origin xyz="0 0 0.046" rpy="${90.0 * deg_to_rad} ${-90.0 * deg_to_rad} ${180 * deg_to_rad}" />
	</joint>

	<joint name="${name}_grasping_frame_joint" type="fixed">
	<parent link="${name}_link" />
	<child link="${name}_grasping_frame" />
	<origin xyz="0.12 0 0" rpy="0 0 0" />
	</joint>

	<joint name="${name}_joint" type="fixed">
	<insert_block name="origin"/>
	<parent link="${parent}" />
	<child link="${name}_link" />
	<axis xyz="0 0 0" />
	</joint>

	<!-- Gripper -->
	<xacro:include filename="$(find pal_gripper_description)/urdf/gripper.urdf.xacro" />