Ignore fixed joints in RNE

[AdrianZw]

First of all, great work! I already love the robotics-toolbox for python. That is exactly what I was searching for!

Unfortunately, I discovered some issue while loading with a custom URDF model which contains many fixed joint in between the actual joints and calling the recursive Newton Euler algorithm rne for the ERobot class. While checking my implementation for the custom URDF model, I discovered that there are similar problems with the UR5 robot. Unfortunately, the rne methods throws the following exception:

'NoneType' object has no attribute 's'
  File "/home/.../robotics-toolbox-python/roboticstoolbox/robot/ERobot.py", line 1918, in rne
    s[j] = link.v.s

Code to reproduce:

import numpy as np
import roboticstoolbox as rp

ur5 = rp.models.URDF.UR5()
qZero = np.zeros((6,))
print(ur5.rne(qZero, qZero, qZero))

Affected Versions: current master branch ( 98713926988f842474d7cf42d6560d53881bbf7d) & v0.9.1 (installed via pip) Python Version: 3.6.9

I had the same behavior for my custom model.

Apparently, for fixed joints there is no "joint axis projection" s defined. Please consider the changes of my fork. I handled these fixed joints. Due to the fixed joints there will also be more link elements in the ERobot class that is why I changed the for loops of the forward and the backward recursion. It seems that these updates solve the issue for the UR5 model. I added a unit test in which I compare the values to the results of the KDL implementation of the RNE algorithm with the official ur_description package.

Unfortunately, this commit does not fix the issues with my custom model. I don't get an exception anymore, but the torques are always zero. I will try to create a minimum falling example for this model. I guess the initialization in Line 1915 to 1921 is not properly implemented if fixed joint - link combinations shall be supported within the tree.

What is your opinion on this?

Thanks.

[petercorke]

Thanks for your interest in the toolbox. The rne dynamics are strictly for the DHRobot class. For the more general ERobot class the plan was to use a different implementation based on spatial vector notation. The support for that is already in spatial math toolbox but I never got around to finishing the alternative version of inverse dynamics, this is where I got to. It doesn't explicitly handle fixed links either but I think they'll be easier to deal with in this framework.

import numpy as np
from spatialmath import Twist3, SE3
import roboticstoolbox as rtb
from roboticstoolbox import ETS as E
from spatialmath import SpatialAcceleration, SpatialVelocity, SpatialInertia, SpatialForce
from math import pi

import roboticstoolbox as rtb 

# inverse dynamics (recursive Newton-Euler) using spatial vector notation
def  ID( robot, q, qd, qdd ):
    
    n = robot.n

    # allocate intermediate variables
    Xup = SE3.Alloc(n)
    Xtree = SE3.Alloc(n)

    v = SpatialVelocity.Alloc(n)
    a = SpatialAcceleration.Alloc(n)
    f = SpatialForce.Alloc(n)
    I = SpatialInertia.Alloc(n)
    s = [None for i in range(n)]   # joint motion subspace
    Q = np.zeros((n,))   # joint torque/force

    # initialize intermediate variables
    for j, link in enumerate(robot):
        I[j] = SpatialInertia(m=link.m, r=link.r)
        Xtree[j] = link.Ts
        s[j] = link.v.s

    a_grav = SpatialAcceleration(robot.gravity)
    
    # forward recursion
    for j in range(0, n):
        vJ = SpatialVelocity(s[j] * qd[j])

        # transform from parent(j) to j
        Xup[j] = robot[j].A(q[j]).inv()

        if robot[j].parent is None:
            v[j] = vJ
            a[j] = Xup[j] * a_grav + SpatialAcceleration(s[j] * qdd[j])
        else:
            jp = robot[j].parent.jindex
            v[j] = Xup[j] * v[jp] + vJ
            a[j] = Xup[j] * a[jp] \
                 + SpatialAcceleration(s[j] * qdd[j]) \
                 + v[j] @ vJ

        f[j] = I[j] * a[j] + v[j] @ (I[j] * v[j])
    
    # backward recursion
    for j in reversed(range(0, n)):
        Q[j] = f[j].dot(s[j])

        if robot[j].parent is not None:
            jp = robot[j].parent.jindex
            f[jp] = f[jp] + Xup[j] * f[j]

    return Q
    
if __name__ == "__main__":

    #robot = rtb.models.URDF.UR5()
    l1 = rtb.ELink(ets=E.ry(), m=1, r=[0.5, 0, 0], name='l1')
    l2 = rtb.ELink(ets=E.tx(1) * E.ry(), m=1, r=[0.5, 0, 0], parent=l1, name='l2')
    robot = rtb.ERobot([l1, l2], name="simple 2 link")

    print(robot)

    print(robot.dyntable())
    print(robot.gravity)
    print(robot[1].v)
    robot.dyntable()

    z = np.zeros(robot.n)

    # should be [2g 0.5g]
    print(robot.fkine([0,0]))
    tau = ID(robot, [0, 0], z, z)
    print('tau', tau)

    # should be [1.5g 0]
    tau = ID(robot, [0, -pi/2], z, z)
    print('tau', tau)

    # should be [1.5g 0]
    #tau = ID(robot, [pi/2, -pi/2], [2,-3], [3,4])
    tau = ID(robot, [0, -pi/2], z, z)
    print('tau', tau)
    robot.gravity=[0,0,0]
    tau = ID(robot, [0, -pi/2], [1,1], z)
    print('tau', tau)

    # should be [0 0]
    tau = ID(robot, [pi/2, 0], z, z)
    print('tau', tau)