Proposed fixes for RNE with fixed joints give incorrect results

[adriencholet]

As previously reported in #236 #368 #408 and commit a53c864 in the future branch, there is an issue with links with fixed transforms when running any function that uses the rne function.

I am working with Python 3.12, rtb 1.1.1 and windows 11 OS.

The issue and error codes are described in #368, I have tried the fixes proposed in #408 and commit a53c864 and I can confirm that the error no longer appears. Unfortunately, uppon closer inspection, I noticed that the results are incorrect.

What should be expected from inverse dynamics:
Consider a robot like the Kinova GEN3 - 7DOF robot (as an example) in a static configuration were its shoulder joint is at 90° and all other joints are at 0°, we can expect that this shoulder joint will need to provide a certain amount of torque to fight gravity. We should also expect that, in static (no acceleration/speed), the inverse dynamics to assess the torque at the shoulder joint could be assessed by fixing all other joints to their value.

What I tested:

I tested the rne algorithm with the PR #408 with a robot loaded from the kinova gen3-7DOF urdf (rtb_data) in two conditions:

1. loading the normal urdf, q = [0,pi/2,0,0,0,0,0] and q' = q'' = np.zeros(7)
   The result is tau = [-1.72500053e-04, -2.34919616e+01, 2.99904203e-01, -7.10429044e+00,
   -5.16569518e-02, -9.69104768e-01, -5.59939070e-02]
   Note that the shoulder joint sees -23.5N.m

2. changing the type of the 3 joint (named Actuator3 in the urdf file) to fixed directly in the urdf and loading this modified urdf, q = [0,pi/2,0,0,0,0] and q' = q'' = np.zeros(6) (The robot becomes 6DoF)
   The result is tau = [-1.80812882e-04 -2.46235114e+01 -7.18009690e+00 -5.16569518e-02
   -9.69104768e-01 -5.59939070e-02]
   Note that the shoulder joint now sees -24.6 N.m but the modification should not have changed anything.

Comparing with pinocchio as a reference,
Both results should return -2.35 for both cases.

I looked into the PR and I noticed that the center of mass and inertia matrix of the link after the fixed joint were not transformed. This is the modifications I propose:

1. When initializing intermediate variables, the inertia and center of mass of links following fixed joints should be transformed to be in the same referential as the previous joint.
2. In the forward recursion, the transformation matrix should be updated to take into account the transforms of fixed links

The resulting fix is the following:

def rne(self,
            q: NDArray,
            qd: NDArray,
            qdd: NDArray,
            symbolic: bool = False,
            gravity: Union[ArrayLike, None] = None,
            ):
        n = self.n

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

        v = SpatialVelocity.Alloc(n)
        a = SpatialAcceleration.Alloc(n)
        f = SpatialForce.Alloc(n)
        I = SpatialInertia.Alloc(n)  # noqa
        s = [[]] * n  # joint motion subspace

        # Handle trajectory case
        q = getmatrix(q, (None, None))
        qd = getmatrix(qd, (None, None))
        qdd = getmatrix(qdd, (None, None))
        l, _ = q.shape  # type: ignore

        if symbolic:  # pragma: nocover
            Q = np.empty((l, n), dtype="O")  # joint torque/force
        else:
            Q = np.empty((l, n))  # joint torque/force

        for k in range(l):
            qk = q[k, :]
            qdk = qd[k, :]
            qddk = qdd[k, :]

            # initialize intermediate variables
            for link in self.links:
                # Find closest joint
                reference_link = link
                transform_matrix = SE3()
                while reference_link is not None and reference_link.jindex is None:
                    transform_matrix = transform_matrix * SE3(reference_link.A())
                    reference_link = reference_link.parent

                # This means we are linked to the base link w/o joints inbetween
                # Meaning no inertia to be computed
                if reference_link is None or reference_link.jindex is None:
                    continue

                # Adding inertia of fixed links
                joint_idx = reference_link.jindex

                transformed_r = (transform_matrix * link.r).flatten()

                transform_matrix = np.array(transform_matrix)
                t = transform_matrix[3,:3]
                rot = transform_matrix[:3,:3]
                transformed_I = rot@(link.I + link.m * (np.dot(t, t) * np.eye(3) - np.outer(t, t)))@rot.T

                I.data[joint_idx] = I[joint_idx].A + \
                                    (SpatialInertia(m=link.m, r=transformed_r, I=transformed_I).data[0])

                # Get joint subspace
                if link.v is not None:
                    s[link.jindex] = link.v.s

            if gravity is None:
                a_grav = -SpatialAcceleration(self.gravity)
            else:  # pragma nocover
                a_grav = -SpatialAcceleration(gravity)

            # forward recursion
            for j in range(0, n):
                vJ = SpatialVelocity(s[j] * qdk[j])

                # Find link attached to joint j
                current_link = None
                for current_link in self.links:
                    # We have already added the inertia of all fixed
                    # links connected to this joint so we can consider
                    # only the link closest to the joint
                    if current_link.jindex == j:
                        break
                if current_link is None:
                    raise ValueError(f"Joint index {j} not found in "
                                     f"robot {self.name}, is the model correct?")

                previous_link = current_link.parent
                j_previous = previous_link.jindex

                # transform to previous joint
                Xup[j] = SE3(current_link.A(qk[current_link.jindex]))
                while j_previous is None:
                    # if previous joint is fixed, add transform to joint before until previous non-fixed joint
                    Xup[j] = SE3(previous_link.A(qk[current_link.jindex]))*Xup[j]
                    previous_link = previous_link.parent
                    if previous_link is None:
                        break
                    j_previous = previous_link.jindex

                Xup[j] = Xup[j].inv()

                # compute velocity and acceleration
                if j_previous is None:
                    v[j] = vJ
                    a[j] = Xup[j] * a_grav + SpatialAcceleration(s[j] * qddk[j])
                else:
                    v[j] = Xup[j] * v[j_previous] + vJ
                    a[j] = Xup[j] * a[j_previous] + \
                           SpatialAcceleration(s[j] * qddk[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 dot(), but fails for symbolic arguments
                Q[k, j] = sum(f[j].A * s[j])

                # Find link attached to joint j
                current_link = None
                for current_link in self.links:
                    if current_link.jindex == j:
                        break
                if current_link is None:
                    raise ValueError(f"Joint index {j} not found in "
                                     f"robot {self.name}, is the model correct?")

                # Find previous joint
                previous_link = current_link.parent
                j_previous = None
                while j_previous is None:
                    if previous_link is None:
                        break
                    j_previous = previous_link.jindex
                    previous_link = previous_link.parent

                # Compute backward pass
                if j_previous is not None:
                    f[j_previous] = f[j_previous] + Xup[j] * f[j]
        if l == 1:
            return Q[0]
        else:  # pragma nocover
            return Q

I verified this fix with Pinocchio and obtain the same results so I think this should be correct !

The issue with commit a53c864 of the future branch is the same, com and inertia matrix of the links after fixed joint are not transformed.

I hope this helps !