Forward dynamics for EROBOT class is giving errors

[kumar10725]

Check here first

Common issues

Describe the bug
Forward kinematics calculation for ERobot class raises error for rne and gravload calculations. The urdf is defined with inertial tags however, Error is not easy to understand.

Version information

Did you install from PyPI or GitHub?
Github

Robotics Toolbox depends heavily on two other packages: Swift (3D graphics) and SpatialMath toolbox (underpinning maths utilities). If you think your issue is related to these, then please answer the questions above for them.

To Reproduce
Steps to reproduce the behavior:

gb12 = get_gb12()
print(gb12)
print(gb12.elinks)

print(gb12.gravload(np.array([0,0,0,0.1,0.1,0])))
print(gb12.rne(np.array([0,0,0,0.1,0.1,0]), np.zeros((6,)), np.zeros((6,))))

Output is as follow:

ERobot: GP12 Robot.SLDASM, 6 joints (RRRRRR), dynamics, geometry, collision
┌───┬───────────┬───────┬───────────┬────────────────────────────────────────────────────────┐
│id │   link    │ joint │  parent   │                          ETS                           │
├───┼───────────┼───────┼───────────┼────────────────────────────────────────────────────────┤
│ 1 │ base_link │       │ BASE      │ {base_link} = {BASE}                                   │
│ 2 │ link_1_s  │     0 │ base_link │ {link_1_s} = {base_link} ⊕ tz(0.45) ⊕ Rz(q0)           │
│ 3 │ link_2_l  │     1 │ link_1_s  │ {link_2_l} = {link_1_s} ⊕ tx(0.155) ⊕ Ry(q1)           │
│ 4 │ link_3_u  │     2 │ link_2_l  │ {link_3_u} = {link_2_l} ⊕ tz(0.614) ⊕ Ry(-q2)          │
│ 5 │ link_4_r  │     3 │ link_3_u  │ {link_4_r} = {link_3_u} ⊕ tx(0.64) ⊕ tz(0.2) ⊕ Rx(-q3) │
│ 6 │ link_5b   │     4 │ link_4_r  │ {link_5b} = {link_4_r} ⊕ Ry(-q4)                       │
│ 7 │ link_6_t  │     5 │ link_5b   │ {link_6_t} = {link_5b} ⊕ Rx(-q5)                       │
│ 8 │ @tool     │       │ link_6_t  │ {tool} = {link_6_t} ⊕ tx(0.1) ⊕ Ry(-90°) ⊕ Rx(180°)    │
└───┴───────────┴───────┴───────────┴────────────────────────────────────────────────────────┘

[ELink(base_link, ets=, m=22.8, r=[-0.0505, 0.00297, 0.0574], I=[0.246, 0.328, 0.471, 0.0107, 0.0256, -0.00521]), ELink(link_1_s, ets=tz(0.45) ⊕ Rz(q), parent=base_link, qlim=[-2.97, 2.97], m=44.7, r=[0.0349, 0.00545, -0.188], I=[0.662, 0.833, 0.887, 0.0343, -0.205, 0.0295]), ELink(link_2_l, ets=tx(0.155) ⊕ Ry(q), 
parent=link_1_s, qlim=[-1.57, 2.71], m=30.9, r=[-0.0177, -0.092, 0.258], I=[0.901, 0.946, 0.0674, -0.00243, 0.0105, 0.027]), ELink(link_3_u, ets=tz(0.614) ⊕ Ry(-q), parent=link_2_l, qlim=[-1.48, 2.62], m=10.5, r=[0.0895, -0.0341, 0.12], I=[0.0749, 0.103, 0.0591, -0.000959, -0.031, 0.00245]), ELink(link_4_r, ets=tx(0.64) ⊕ tz(0.2) ⊕ Rx(-q), parent=link_3_u, qlim=[-3.14, 3.14], m=4.95, r=[-0.224, -0.000363, 0.017], I=[0.0169, 0.0635, 0.0665, -0.00261, 0.0016, -0.000299]), ELink(link_5b, ets=Ry(-q), parent=link_4_r, qlim=[-2.62, 2.62], m=2.1, r=[0.0263, -0.0151, -0.000336], I=[0.00389, 0.00385, 0.00396, 6.02e-05, -5.56e-06, 6.05e-05]), ELink(link_6_t, ets=Rx(-q), parent=link_5b, qlim=[-6.28, 6.28], m=0.00249, r=[0.0915, 5.42e-18, -1.11e-15], I=[6.92e-06, 3.46e-06, 3.46e-06, 4.75e-22, -2.49e-21, 1.55e-35]), ELink(tool, ets=tx(0.1) ⊕ Ry(-90°) ⊕ Rx(180°), parent=link_6_t, m=0.615, r=[3.15e-05, 1.76e-09, 0.0127], I=[0.000564, 0.000555, 0.00108, 5.8e-06, 1.78e-07, 1.96e-13])]
Traceback (most recent call last):
  File "c:/Users/ASUS/Desktop/robot2021/DOE/testere.py", line 52, in <module>
    print(gb12.gravload(np.array([0,0,0,0.1,0.1,0])))
  File "c:\users\asus\desktop\robot2021\doe\robotics-toolbox-python\roboticstoolbox\robot\Dynamics.py", line 850, in gravload     
    taug[k, :] = self.rne(qk, z, z, gravity=gravity)
  File "c:\users\asus\desktop\robot2021 \doe\robotics-toolbox-python\roboticstoolbox\robot\ERobot.py", line 2100, in rne
    I[j] = SpatialInertia(m=link.m, r=link.r)
  File "C:\ProgramData\Anaconda3\envs\robotic_env\lib\site-packages\spatialmath\baseposelist.py", line 328, in __setitem__
    self.data[i] = value.A
IndexError: list assignment index out of range

Expected behavior
It should be able to give the rne and gravload values.

Environment (please complete the following information):

• Your OS (MacOS, Linux, Windows). Windows
• Your Python version. 3.7.10

Additional context
Add any other context about the problem here.

[kumar10725]

This function still gives error after defining the world coordinate frame and rne function throws error as follow. The ELINKS are defined and inertia values are valid. However, still the error. I believe it has something to do with the way links and joints are numbered/indexed.

┌───┬───────────┬───────┬───────────┬────────────────────────────────────────────────────────┐
│id │   link    │ joint │  parent   │                          ETS                           │
├───┼───────────┼───────┼───────────┼────────────────────────────────────────────────────────┤
│ 1 │ base_link │       │ BASE      │ {base_link} = {BASE}                                   │
│ 2 │ link_1_s  │     0 │ base_link │ {link_1_s} = {base_link} ⊕ tz(0.45) ⊕ Rz(q0)           │
│ 3 │ link_2_l  │     1 │ link_1_s  │ {link_2_l} = {link_1_s} ⊕ tx(0.155) ⊕ Ry(q1)           │
│ 4 │ link_3_u  │     2 │ link_2_l  │ {link_3_u} = {link_2_l} ⊕ tz(0.614) ⊕ Ry(-q2)          │
│ 5 │ link_4_r  │     3 │ link_3_u  │ {link_4_r} = {link_3_u} ⊕ tx(0.64) ⊕ tz(0.2) ⊕ Rx(-q3) │
│ 6 │ link_5_b  │     4 │ link_4_r  │ {link_5_b} = {link_4_r} ⊕ Ry(-q4)                      │
│ 7 │ link_6_t  │     5 │ link_5_b  │ {link_6_t} = {link_5_b} ⊕ Rx(-q5)                      │
│ 8 │ @tool0    │       │ link_6_t  │ {tool0} = {link_6_t} ⊕ tx(0.1) ⊕ Ry(-90°) ⊕ Rx(180°)   │
│ 9 │ @base     │       │ base_link │ {base} = {base_link} ⊕ tz(0.45)                        │
└───┴───────────┴───────┴───────────┴────────────────────────────────────────────────────────┘

[ELink(base_link, ets=, m=22.8, r=[-0.0505, 0.00297, 0.0574], I=[0.246, 0.328, 0.471, 0.0107, 0.0256, -0.00521]), ELink(link_1_s, ets=tz(0.45) ⊕ Rz(q), parent=base_link, qlim=[-2.97, 2.97], m=44.7, r=[0.0349, 0.00545, -0.188], I=[0.662, 0.833, 0.887, 0.0343, -0.205, 0.0295]), ELink(link_2_l, ets=tx(0.155) ⊕ Ry(q), parent=link_1_s, qlim=[-1.57, 2.71], m=30.9, r=[-0.0177, -0.092, 0.258], I=[0.901, 0.946, 0.0674, -0.00243, 0.0105, 0.027]), ELink(link_3_u, ets=tz(0.614) ⊕ Ry(-q), parent=link_2_l, qlim=[-1.48, 2.62], m=10.5, r=[0.0895, -0.0341, 
0.12], I=[0.0749, 0.103, 0.0591, -0.000959, -0.031, 0.00245]), ELink(link_4_r, ets=tx(0.64) ⊕ tz(0.2) ⊕ Rx(-q), parent=link_3_u, qlim=[-3.49, 3.49], m=4.95, r=[-0.224, -0.000363, 0.017], I=[0.0169, 0.0635, 0.0665, -0.00261, 0.0016, -0.000299]), ELink(link_5_b, ets=Ry(-q), parent=link_4_r, qlim=[-2.62, 2.62], m=2.1, r=[0.0263, -0.0151, -0.000336], I=[0.00389, 0.00385, 0.00396, 6.02e-05, -5.56e-06, 6.05e-05]), ELink(link_6_t, ets=Rx(-q), parent=link_5_b, qlim=[-7.94, 7.94], m=0.00249, r=[0.0915, 5.42e-18, -1.11e-15], I=[6.92e-06, 3.46e-06, 3.46e-06, 4.75e-22, -2.49e-21, 1.55e-35]), ELink(tool0, ets=tx(0.1) ⊕ Ry(-90°) ⊕ Rx(180°), parent=link_6_t), ELink(base, ets=tz(0.45), parent=base_link)]
Traceback (most recent call last):
  File "c:/Users/ASUS/Desktop/robot/DOE/testere.py", line 58, in <module>
    print(gb12.rne(qt.q, qt.qd, qt.qdd))
  File "c:\users\asus\desktop\robot\doe\robotics-toolbox-python\roboticstoolbox\robot\ERobot.py", line 2100, in rne
    I[j] = SpatialInertia(m=link.m, r=link.r)
  File "C:\ProgramData\Anaconda3\envs\robotic_env\lib\site-packages\spatialmath\baseposelist.py", line 330, in __setitem__
    self.data[i] = value.A
IndexError: list assignment index out of range

[kumar10725]

I got a quick fix by making some modifications in the EROBOT.py -> rne funtion.
this only works for the robots that has urdf definitions with a base. Because this library read such URDF models as 7 links and 6 joints (for a 6dof robot). The rne function throws an error for the base link. the modification only considers the link_1 to link_6 for the computation. i have no way to verify if the results are correct but someone might be able to benefit from this.

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

        # initialize intermediate variables : using 1 as starting index considering there is a base_link and n+1 considering presence 
         #of a tool
        for j, link in enumerate(robot.links[1:n+1]):
            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] = SE3(link.Ts, check=False)
            if link.v is not None:
                s[j] = link.v.s
            else:
                s[j] = None
        
        # print("Xtree:::",Xtree)
        # print("S::", s)
        

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

        # forward recursion
        for j in range(0, n):
            print(q)
            print(qd[j],q[j], s[j])
            vJ = SpatialVelocity(s[j] * qd[j])
            # print(robot[j+1].A(q[j]).inv())
            # transform from parent(j) to j
            Xup[j] = robot[j+1].A(q[j]).inv()

            if robot[j+1].parent is None:
                v[j] = vJ
                a[j] = Xup[j] * a_grav + SpatialAcceleration(s[j] * qdd[j])
            else:
                jp = j#robot[j+1].parent.jindex
                # print("Jp = ",jp)
                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)):

            # next line could be np.dot(), but fails for symbolic arguments
            Q[j] = np.sum(f[j].A * s[j])

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

        return Q

[jhavl]

This is related to #236, work in progress.

[jhavl]

Progress on this will be reported in #236