Free joint representation and Jacobian

[Nickick-ICRS]

Hi,

Sorry this isn't so much an issue as a general question about implementation details and mathematical background. I am not aware of a better place to ask this question however (wouldn't it be great if there was a DART discord server haha), so I am asking it here.

I've been looking into the implementation details for various joints, as I'm working very closely with Featherstone's articulated rigid body algorithms in my research. I'm currently able to get very close to, but not quite matching DART's results with my own custom, linearised version of the algorithm when simulating my robot, but the errors (compared to DART's implementation) increase significantly as the transform from the origin to the base of my robot increases (particularly rotations), which is represented as a 6-dof free joint.

Obviously there are other ways of representing this, e.g. a pure SE3 matrix logarithm, but DART's implementation seems to have the free joint generalised position vector as the tangent space to the rotation (using the SO3 matrix logarithmic map and exponential map to convert between them) and the translation with respect to the parent body frame (with no conversion necessary). This corresponds simply to a rotation about the parent frame, followed by a translation, again with respect to the parent frame. Is this correct? DART then uses a fixed joint jacobian matrix equal to the adjoint of the transform of the child from the joint connection point, with the derivative equal to zero.

In Featherstone's book, Rigid Body Dynamics Algorithms, page 81, example 4.5:

A peculiar feature of both the planar joint and the 6-DoF joint in Table 4.1 is that the translational joint variables express the translation in Fs coordinates rather than Fp coordinates. The reason for doing this is to make the motion subspace matrix a simple constant, so that ˚S = 0, cJ = 0, and the simple form of S allows various optimizations to be applied to the dynamics algorithms.

That is, in order for the joint jacobian derivative to be zero, the joint parameterisation of the free joint should include the translation of the parent to the child, but in the coordinates of the child frame rather than the coordinates of the parent frame.

This leads to my question: Have I made a mistake in my understanding of the DART implementation, or the mathematics behind it? Or is the effect of the derivative of the joint jacobian minimal enough in stable simulation cases that a slight error here doesn't matter? Or can the jacobian derivative still be zero (and jacobian fixed) if the translation component is still given in the coordinates of the parent frame? The latter does not seem to be true later on in Featherstone's book, but using the child-frame translations instead makes calculating velocities from them that much harder.

[Nickick-ICRS]

The main relevant files of course being:
https://github.com/dartsim/dart/blob/main/dart/dynamics/FreeJoint.hpp
https://github.com/dartsim/dart/blob/main/dart/dynamics/FreeJoint.cpp

And functions:

convertToPositions
convertToTransform
updateRelativeJacobian
updateRelativeJacobianTimeDeriv

[jslee02]

Thank you for your detailed question. I may need time to refresh the current implementation details and mathematical rationale. I'll aim to provide a more informed response by this weekend.

[Nickick-ICRS]

I've spent way too long writing out maths equations and scratching my head, so I decided to just test this directly with some simple code on a single rigid body:

#include <dart/dart.hpp>

namespace dd = dart::dynamics;
namespace ds = dart::simulation;

int main() {
    double dt = 0.01;

    auto world = ds::World::create();
    auto skel = dd::Skeleton::create();
    world->setTimeStep(dt);
    // Testing without any acceleration effects
    world->setGravity(Eigen::Vector3d::Zero());
    auto body_and_jnt = skel->createJointAndBodyNodePair<dd::FreeJoint>(nullptr);
    auto jnt = body_and_jnt.first;
    auto body = body_and_jnt.second;
    body->setMomentOfInertia(1., 1., 1.);
    body->setMass(1.);
    world->addSkeleton(skel);

    double yaw = M_PI_4;
    double x_pos = 1000.;
    double x_vel = 1.;

    Eigen::Vector6d X_0, V_0;
    X_0 << 0., 0., yaw, x_pos, 0., 0.;
    V_0 << 0., 0., 0., x_vel, 0., 0.;

    jnt->setPositions(X_0);
    jnt->setVelocities(V_0);

    for (size_t i = 0; i < 100; i++) {
        world->step(false);
        // True if X is stored in the world frame
        assert(dd::FreeJoint::convertToTransform(jnt->getPositions()).isApprox(body->getWorldTransform()));
        // True if V is stored in local coordinates
        assert(jnt->getVelocities().isApprox(body->getSpatialVelocity(dd::Frame::World(), body)));
        // True if V is stored in world coordinates
        // Fails
        //assert(jnt->getVelocities().isApprox(body->getSpatialVelocity(dd::Frame::World(), dd::Frame::World())));
    }

    std::cerr << "Initial X: " << X_0.transpose() << std::endl;
    std::cerr << "  Final X: " << jnt->getPositions().transpose() << std::endl;
    std::cerr << "Initial V: " << V_0.transpose() << std::endl;
    std::cerr << "  Final V: " << jnt->getVelocities().transpose() << std::endl;

    std::cerr << "DART VERSION " << DART_VERSION << std::endl;
}

Since we start with a yaw of 90 degrees, a 1 unit translation along X in the child frame would be a 1 unit translation along Y in the parent frame, which is aligned with the world. Equally, a 1 unit translation along X in the parent frame, would be a 1 unit translation along -Y in the child frame.

Since the final output is:

Initial X: 0 0 1.5708 1000 0 0
Final X: 0 0 1.5708 1000 1 0
Initial V: 0 0 0 1 0 0
Final V: 0 0 0 1 0 0
DART VERSION 6.11.2

We can see that DART must store the (linear) position in the parent coordinate frame, and the (linear) velocity in the child coordinate frame. Which can be seen more easily when a yaw velocity is added.

In my notation the top left superscript is the coordinate frame that the variable is expressed in, so ${}^p\mathbf{x}$ would be the translation component expressed in the parent frame ($p$) rather than the child frame ($c$). $\mathbb{I}$ is the identity matrix, and the so3/se3 hat map of a vector $\mathbf{x}$ is written as $\hat{\mathbf{x}}$.

If we store the position of the child with respect to the parent in the coordinates of the parent frame, this is equivalent to a rotation about the origin, and then a translation.

$$\begin{align} let\ &\mathcal{X} \in \mathbb{R}^6 := \begin{pmatrix} \theta \\\ {}^p\mathbf{x} \end{pmatrix} \\\ \therefore\ &\hat{\mathcal{X}} \in \mathfrak{se}(3) \end{align}$$

Then

$$\mathbf{T}(\mathcal{X}) \in SE(3) := \begin{pmatrix} \mathbb{I} & {}^p\mathbf{x} \\\ 0 & 1 \end{pmatrix} \begin{pmatrix} e^{\hat{\theta}} & \mathbf{0} \\\ 0 & 1 \end{pmatrix} = \begin{pmatrix} e^{\hat{\theta}} & {}^p\mathbf{x} \\\ 0 & 1 \end{pmatrix}$$

Which is what DART calculates in the Eigen::Isometry3d FreeJoint::convertToTransform(Eigen::Vector6d p) function, and in my simple example matches the world transform as well (by not asserting).

Reading through Featherstone's one more time, straight after the example I quoted above:

However, there is potentially a big disadvantage in this
arrangement: if the successor body is far distant from the predecessor, and
starts to spin, then the spinning will cause large rates of change in the position
variables. (Just think of a tumbling satellite or aircraft.) One way to get the
best of both worlds is to use a set of position variables in which the translations
are expressed in Fp coordinates, together with a set of velocity variables in
which the linear velocity is expressed in Fs coordinates.

So DART is using the same system as Featherstone as expected. One thing I noticed, is that manually deriving the transform between the two units gives a different result to what DART seems to use:

$$\begin{aligned} {}^p\mathbf{x} &= {}_c\mathbf{R}_p {}^c\mathbf{x} \\\ {}^p\dot{\mathbf{x}} &= {}_c\mathbf{R}_p {}_c\dot{\mathbf{R}}_p {}^c\mathbf{x} + {}_c\mathbf{R}_p {}^c\dot{\mathbf{x}} \end{aligned}$$

While DART seems to do:

$${}^p\dot{\mathbf{x}} = {}_c\mathbf{R}_p {}^c\dot{\mathbf{x}}$$

I'm guessing this is because we're actually storing the velocity of the body relative to the instantaneous axis-aligned origin coincident with the rigid body / child frame rather than the actual (maybe very far away) parent frame, so therefore ${}^c\mathbf{x}$ is always zero.