ReflectedInertiaTreeModel with minimal coordinate != link joint

[nicholasadr]

Currently ReflectedInertiaTreeModel only account for the case when the link joint is the minimal coordinate. This might not always be the case, for example, in Tello Differential where the minimal coordinate is the post-gearbox output of the rotor.

In order to rectify this, we need to project vectors/matrices that are expressed in link joints to the chosen minimal coordinate. For example, let's take a look at forwardDynamicsHinv() implementation:

308: DVec<double> ReflectedInertiaTreeModel::forwardDynamicsHinv(const DVec<double> &tau)
309:     {
310:          compositeRigidBodyAlgorithm();
311:           updateBiasForceVector();
312:          DVec<double> qdd_ref_inertia = (H_ + reflected_inertia_).inverse() * (tau - C_);
313:          return qdd_ref_inertia;
314:      }

Performing projection will change line 312 into something like this:
312: DVec<double> qdd_ref_inertia = (J.transpose()*H_*J + reflected_inertia_).inverse() * (tau - J.transpose()*(C_ + H_*j))
where:

• qdot_link = J * ydot_minimal_coordinate
• qddot_link = J*yddot_minimal_coordinate + j
• reflected_inertia_ is assumed to be already in minimal coordinate when it is defined in GeneralizedJoint
• tau is assumed to be defined in minimal coordinate

Meanwhile, looking at forwardDynamicsABA() second pass:

413: link_node->U_ = link_node->IA_ * joint->S();
414: DMat<double> D = joint->S().transpose() * link_node->U_;
415: if (use_reflected_inertia)
416:      D += reflected_inertia_.block(vel_idx, vel_idx, num_vel, num_vel)

We would need to modify lines 413-414 to:

413: link_node->U_ = link_node->IA_ * joint->S() * J_i;
414: DMat<double> D = J_i.transpose() * joint->S().transpose() * link_node->U_;

Some of the challenges in this fix include:

• including LoopConstraint class as part of ReflectedInertiaTreeModel
• ReflectedInertiaTreeModel is initialized with independent joint coordinate, but we potentially need the spanning tree coordinate to update the LoopConstraint class
• obtaining J requires complex indexing of loop_constraint_.G_
• similarly, obtaining j requires complex indexing of loop_constraint_.g_

[MatthewChignoli]

What do you mean by complex indexing of G and g?

[nicholasadr]

In order to get J or J_i, we need to select the rows of G that correspond to the link joints. So there's a need to track which row in each cluster's G that corresponds to the link joints. It's not clear to me at the moment on the cleanest way to do this.

[MatthewChignoli]

Did you start working on this problem? I want to include the Tello model in one of our accuracy benchmarks, and I think it relies on getting this right

[nicholasadr]

I did start looking into that but had to put it on hold to prioritize other things that came up. I also remember I was stuck on something in the code but I can take a look at this again.