robot.rne function does not work for robots with static joints

[Adineh94]

Hi,
I have created two robots from build-in functions, as following. However, I am not able to get "inertia" matrix for robot which has been created from URDF, and I have received this error:

File "C:\Users\moham\miniconda3\envs\rtb\lib\site-packages\roboticstoolbox\robot\ERobot.py", line 2102, in rne
s[j] = link.v.s
AttributeError: 'NoneType' object has no attribute 's'
#############Code#########################
import roboticstoolbox as rtb
panda = rtb.models.URDF.Panda()
puma = rtb.models.DH.Puma560()

print(puma.inertia(puma.qr))
print(panda.inertia(panda.qr))

[petercorke]

That URDF model has no dynamic parameters set

(rtb) >>> panda.dynamics()

┌────────────┬────┬────────────┬────────────────────────┬────┬────┬────────┬────┐
│     j      │ m  │     r      │           I            │ Jm │ B  │   Tc   │ G  │
├────────────┼────┼────────────┼────────────────────────┼────┼────┼────────┼────┤
│panda_link0 │  0 │  0,  0,  0 │  0,  0,  0,  0,  0,  0 │  0 │  0 │  0,  0 │  0 │
│panda_link1 │  0 │  0,  0,  0 │  0,  0,  0,  0,  0,  0 │  0 │  0 │  0,  0 │  0 │
│panda_link2 │  0 │  0,  0,  0 │  0,  0,  0,  0,  0,  0 │  0 │  0 │  0,  0 │  0 │
│panda_link3 │  0 │  0,  0,  0 │  0,  0,  0,  0,  0,  0 │  0 │  0 │  0,  0 │  0 │
│panda_link4 │  0 │  0,  0,  0 │  0,  0,  0,  0,  0,  0 │  0 │  0 │  0,  0 │  0 │
│panda_link5 │  0 │  0,  0,  0 │  0,  0,  0,  0,  0,  0 │  0 │  0 │  0,  0 │  0 │
│panda_link6 │  0 │  0,  0,  0 │  0,  0,  0,  0,  0,  0 │  0 │  0 │  0,  0 │  0 │
│panda_link7 │  0 │  0,  0,  0 │  0,  0,  0,  0,  0,  0 │  0 │  0 │  0,  0 │  0 │
│panda_link8 │  0 │  0,  0,  0 │  0,  0,  0,  0,  0,  0 │  0 │  0 │  0,  0 │  0 │
└────────────┴────┴────────────┴────────────────────────┴────┴────┴────────┴────┘

but that shouldn't lead to the error you report (which I also get). None of our shipped Panda models have dynamic parameters at the moment, but the MATLAB DH model does include inertial parameters.

I'll log this a bug.

[petercorke]

On a quick look this is due to static joints, ie. two links with a constant transform between them. It's not an issue with URDF, it's just that URDF allows us to express this kind of robot structure.

I will need to dig into the originals papers again to sort this out, but that's not likely to happen soon.

[jenskober]

starting from Jesse's fix, here is my attempt at modifying the rne function. It now runs through without any errors for a number of URDF files I tested, but the resulting torque is quite different from an equivalent model using a DH definition. Hope that helps somebody a bit to finally get this working.

    # inverse dynamics (recursive Newton-Euler) using spatial vector notation
    def rne(robot, q, qd, qdd, symbolic=False, gravity=None):

        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)  # noqa
        s = []  # joint motion subspace
        if symbolic:
            Q = np.empty((n,), dtype="O")  # joint torque/force
        else:
            Q = np.empty((n,))  # joint torque/force

        # A temp variable to handle static joints
        Ts = SE3(np.eye(4, dtype="O"), check=False)

        # A counter through joints
        j = 0

        # initialize intermediate variables
        for link in robot.links:
            if link.isjoint:
                I[j] = SpatialInertia(m=link.m, r=link.r)
                if symbolic and link.Ts is None:
                    Xtree[j] = SE3(np.eye(4, dtype="O"), check=False)
                else:
                    Xtree[j] = Ts * SE3(link.Ts, check=False)

                if link.v is not None:
                    s.append(link.v.s)

                # Increment the joint counter
                j += 1

                # Reset the Ts tracker
                Ts = SE3(np.eye(4, dtype="O"), check=False)
            else:
                Ts *= SE3(link.Ts, check=False)

        if gravity is None:
            a_grav = -SpatialAcceleration(robot.gravity)
        else:
            a_grav = -SpatialAcceleration(gravity)

        # forward recursion

        # A counter through joints
        j = 0

        for link in robot.links:
            if link.isjoint:
                vJ = SpatialVelocity(s[j] * qd[j])

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

                jointParent = link.parent
                while not (jointParent is None or jointParent.isjoint):
                    jointParent = jointParent.parent

                if jointParent is None:
                    v[j] = vJ
                    a[j] = Xup[j] * a_grav + SpatialAcceleration(s[j] * qdd[j])
                else:
                    jp = jointParent.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])

                # Increment the joint counter
                j += 1

        # backward recursion

        # A counter through joints
        j = n-1

        for link in reversed(robot.links):
            if link.isjoint:
                # next line could be np.dot(), but fails for symbolic arguments
                Q[j] = np.sum(f[j].A * s[j])

                jointParent = link.parent
                while not (jointParent is None or jointParent.isjoint):
                    jointParent = jointParent.parent

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

                # Decrement the joint counter
                j -= 1

        return Q

[Betancourt20]

Hi,
I tried to obtain the inertial matrix of the URDF of the Puma560 robot but I did not succeed. I implement the proposed solution of jenskober in the last version of the master (v1.0.2) but I could not make it possible to extract the inertial matrix of the URDF of Puma560. The following error is with and without the proposed solution.

Traceback (most recent call last):
  File "/robotics-toolbox-python/roboticstoolbox/robot/ET.py", line 505, in A
    return ET_T(self.__fknm, q)
TypeError: Symbolic value

During handling of the above exception, another exception occurred:

Traceback (most recent call last):
  File "/URDF/Stablity.py", line 21, in <module>
    M = Puma.inertia(q)
  File "/robotics-toolbox-python/roboticstoolbox/robot/Dynamics.py", line 750, in inertia
    In[k, :, :] = self.rne(
  File "/robotics-toolbox-python/roboticstoolbox/robot/ERobot.py", line 2102, in rne
    Xup[j] = link.A(q[j]).inv()
  File "/robotics-toolbox-python/roboticstoolbox/robot/Link.py", line 1431, in A
    return SE3(self._Ts @ self._ets[-1].A(q), check=False)
  File "/robotics-toolbox-python/roboticstoolbox/robot/ET.py", line 513, in A
    return self.axis_func(q)
  File "/spatialmath-python/spatialmath/base/transforms3d.py", line 203, in troty
    T = smb.r2t(roty(theta, unit))
  File "/spatialmath-python/spatialmath/base/transforms3d.py", line 95, in roty
    ct = smb.sym.cos(theta)
  File "/spatialmath-python/spatialmath/base/symbolic.py", line 134, in cos
    return math.cos(theta)
TypeError: only size-1 arrays can be converted to Python scalars

[niederha]

Hello!
I have had this problem, and made a PR to fix it: #408 I'm hoping to get it looked at so I thought I would ask here. Or at least the next person looking at this issue might have an idea how to circumvent the problem,