Joint Axis is rotated when converted to KDL?

[JohnSilverFarmer]

Hi,

I'm trying to normalize a set of URDF files so that the joint axis always aligns with the z-axis. The conversion is done by changing the joint axis and than adding an addition rotation in the joint origin. When visualizing this in e.g. rviz this seems to be working correctly. However, when I now try to parse the changed URDF to KDL the joint axis is rotated back to the original axis. This is due to the rotation in line 54 of urdf.py:
rotational = lambda j,F: kdl.Joint(j.name, F.p, F.M * kdl.Vector(*j.axis), kdl.Joint.RotAxis)

Also, when I compute the inverse dynamics for the normalized urdf I get a different result than with the original urdf. However, when I do the inverse dynamics computation in pyBullet (and use their urdf parser) I get the same result for the two urdfs.

Why is the joint axis rotated when parsing the URDF? Is there a way to get around this so that the joint axes in KDL match the joint axes in the URDF?

Cheers,
Johannes

Example Code

from kdl_parser_py import urdf
import PyKDL as kdl
import numpy as np
import pybullet


def to_kdl_array(a):
    a_ = kdl.JntArray(len(a))
    for i in range(len(a)):
        a_[i] = a[i]
    return a_


def from_kdl_array(a):
    a_ = []
    for i in range(a.rows()):
        a_.append(a[i])
    return a_


def inv_dyn(chain: kdl.Chain, q: list, qd: list, qdd: list) -> list:
    rne_solver = kdl.ChainIdSolver_RNE(chain, kdl.Vector(0, 0, -9.81))
    tau = to_kdl_array(len(q) * [0])

    rne_solver.CartToJnt(to_kdl_array(q),
                         to_kdl_array(qd),
                         to_kdl_array(qdd),
                         [kdl.Wrench()] * chain.getNrOfSegments(),
                         tau)

    return from_kdl_array(tau)


def rand_config():
    q = [np.random.uniform(-2.356, 2.356)]
    qd = [np.random.uniform(-5, 5)]
    qdd = [np.random.uniform(-3, 3)]
    return q, qd, qdd



def main():
    urdf_joint_y = './exmpl_joint_y.urdf'
    _, tree_joint_y = urdf.treeFromFile(urdf_joint_y)
    chain_joint_y = tree_joint_y.getChain('world', 'pole')

    urdf_joint_z = './exmpl_joint_z.urdf'
    _, tree_joint_z = urdf.treeFromFile(urdf_joint_z)
    chain_joint_z = tree_joint_z.getChain('world', 'pole')

    print(urdf_joint_y)
    for i in range(chain_joint_y.getNrOfSegments()):
        s = chain_joint_y.getSegment(i)
        j = s.getJoint()
        print(s.getName(), '->', j.getName(), '->', j.JointAxis())

    print('\n', urdf_joint_z)
    for i in range(chain_joint_z.getNrOfSegments()):
        s = chain_joint_z.getSegment(i)
        j = s.getJoint()
        print(s.getName(), '->', j.getName(), '->', j.JointAxis())

    q, qd, qdd = rand_config()

    print('\n', urdf_joint_y)
    print('tau kdl ->', inv_dyn(chain_joint_y, q, qd, qdd))

    connect_id = pybullet.connect(pybullet.DIRECT)
    id_robot = pybullet.loadURDF(urdf_joint_y, 3*[0.], 3*[0.] + [1.], useFixedBase=True)
    pybullet.setGravity(0, 0, -9.81)
    print('tau bullet ->', pybullet.calculateInverseDynamics(id_robot, q, qd, qdd))
    pybullet.disconnect(connect_id)

    print('\n', urdf_joint_z)
    print('tau ->', inv_dyn(chain_joint_z, q, qd, qdd))
    _ = pybullet.connect(pybullet.DIRECT)
    id_robot = pybullet.loadURDF(urdf_joint_z, 3 * [0.], 3 * [0.] + [1.], useFixedBase=True)
    pybullet.setGravity(0, 0, -9.81)
    print('tau bullet ->', pybullet.calculateInverseDynamics(id_robot, q, qd, qdd))


if __name__ == '__main__':
    main()

Original URDF

<?xml version="1.0" ?>
<robot name="robot" xmlns:xacro="http://www.ros.org/wiki/xacro">
  <material name="blue">
    <color rgba="0 0 0.9 1"/>
  </material>
  <material name="silver">
    <color rgba="0.75 0.75 0.75 1"/>
  </material>
  <material name="red">
    <color rgba="0.9 0 0 1"/>
  </material>
  <material name="black">
    <color rgba="0 0 0 1"/>
  </material>

  <link name="world"/>

  <link name="arm">
    <visual>
      <geometry>
        <cylinder length="0.085" radius="0.0025"/>
      </geometry>
      <origin rpy="1.5708 0 0" xyz="0 0.0425 0"/>
      <material name="silver"/>
    </visual>
    <collision>
      <geometry>
        <cylinder length="0.085" radius="0.0025"/>
      </geometry>
      <origin rpy="1.5708 0 0" xyz="0 0.0425 0"/>
    </collision>
    <inertial>
      <mass value="0.095"/>
      <origin rpy="1.5708 0 0" xyz="0 0.0425 0"/>
      <inertia ixx="5.73463541667e-05" ixy="0.0" ixz="0.0" iyy="5.73463541667e-05" iyz="0.0" izz="2.96875e-07"/>
    </inertial>
  </link>

  <joint name="world_to_arm" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <parent link="world"/>
    <child link="arm"/>
  </joint>

  <link name="pole">
    <visual>
      <geometry>
        <cylinder length="0.129" radius="0.005"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 -0.06575"/>
      <material name="red"/>
    </visual>
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 -0.0645"/>
      <mass value="0.024"/>
      <inertia ixx="3.3432e-05" ixy="0.0" ixz="0.0" iyy="3.3432e-05" iyz="0.0" izz="3e-07"/>
    </inertial>
    <collision>
      <geometry>
        <cylinder length="0.129" radius="0.005"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 -0.0645"/>
    </collision>
  </link>

  <joint name="arm_to_pole" type="revolute">
    <axis xyz="0 1 0"/>
    <limit effort="10000" lower="-2.356" upper="+2.356" velocity="1000"/>
    <dynamics damping="0.000001" friction="0.000"/>
    <origin rpy="0 0 0" xyz="0 0.085 0.0"/>
    <parent link="arm"/>
    <child link="pole"/>
  </joint>

</robot>

"Normalized" URDF (joint axis is z)

<?xml version="1.0" ?>
<robot name="robot" xmlns:xacro="http://www.ros.org/wiki/xacro">
  <material name="blue">
    <color rgba="0 0 0.9 1"/>
  </material>
  <material name="silver">
    <color rgba="0.75 0.75 0.75 1"/>
  </material>
  <material name="red">
    <color rgba="0.9 0 0 1"/>
  </material>
  <material name="black">
    <color rgba="0 0 0 1"/>
  </material>

  <link name="world"/>

  <link name="arm">
    <visual>
      <geometry>
        <cylinder length="0.085" radius="0.0025"/>
      </geometry>
      <origin rpy="1.5708 0 0" xyz="0 0.0425 0"/>
      <material name="silver"/>
    </visual>
    <collision>
      <geometry>
        <cylinder length="0.085" radius="0.0025"/>
      </geometry>
      <origin rpy="1.5708 0 0" xyz="0 0.0425 0"/>
    </collision>
    <inertial>
      <mass value="0.095"/>
      <origin rpy="1.5708 0 0" xyz="0 0.0425 0"/>
      <inertia ixx="5.73463541667e-05" ixy="0.0" ixz="0.0" iyy="5.73463541667e-05" iyz="0.0" izz="2.96875e-07"/>
    </inertial>
  </link>

  <joint name="world_to_arm" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <parent link="world"/>
    <child link="arm"/>
  </joint>

  <link name="pole">
    <visual>
      <geometry>
        <cylinder length="0.129" radius="0.005"/>
      </geometry>
      <origin rpy="1.5707 0 0" xyz="0 0.06575 0"/>
      <material name="red"/>
    </visual>
    <inertial>
      <origin rpy="1.5707 0 0" xyz="0 0.0645 0"/>
      <mass value="0.024"/>
      <inertia ixx="3.3432e-05" ixy="0.0" ixz="0.0" iyy="3.3432e-05" iyz="0.0" izz="3e-07"/>
    </inertial>
    <collision>
      <geometry>
        <cylinder length="0.129" radius="0.005"/>
      </geometry>
      <origin rpy="1.5707 0 0" xyz="0 0.0645 0"/>
    </collision>
  </link>

  <joint name="arm_to_pole" type="revolute">
    <axis xyz="0 0 1"/>
    <limit effort="10000" lower="-2.356" upper="+2.356" velocity="1000"/>
    <dynamics damping="0.000001" friction="0.000"/>
    <origin rpy="-1.5707 0 0" xyz="0 0.085 0.0"/>
    <parent link="arm"/>
    <child link="pole"/>
  </joint>

</robot>