Split kinematics functions (forward, backward, reconstrution) into their actual input/output relation
Right now, the different kinematics ODEs are set up in a "nested" way i.e., reconstruction
is Q
, r
, \eta
, forward
is Q
, r
, \eta
, \dot{\eta}
, and backward
is Q
, r
, \eta
, , \dot{\eta}
, \Lambda
, Q_{a}
.
Since the backward kinematics are actually only ever needed to determine the F_{0} = \Lambda (0)
and Q_{0} = Q_{a} (0)
, it would make more sense to also only return these respective components from these functions.
As such, these functions are 1:1 implements of their rigid joint counterparts and require no special treatment (even though one could argue that we could further split the result of backward
into backward_force
and backward_actuation
to be fully compatible with rigid joints, however, I believe that a function like [F0, Qa] = backward(joint, Q1, r1, eta1, eta1dot, q, qdot, qddot, F1)
is likely as good as two functions F0 = backward_force(joint, Q1, r1, eta1, eta1dot, q, qdot, qddot, F1)
and Qa = backward_actuation(joint, Q1, r1, eta1, eta1dot, F1, q, qdot, qddot)
.
In the end, we would have
[Q, r, eta] = reconstruction(joint, Q0, r0, eta0, q, qdot, qddot);
[Q, r, eta, etadot] = forward(joint, Q0, r0, eta0, q, qdot, qddot);
[F0, Qa] = backward(joint, Q1, r1, eta1, q, qdot, qddot);
[F0, Qa] = idm(joint, Q0, r0, eta0, eta0dot, q, qdot, qddot, F1);
which, I believe, can provide the same syntax for rigid joints and flexible/soft joints that may be used directly inside the RIDM/RNEA algorithm.