duburcqa
commented
2 years ago After looking more closely at the implementation of the cholesky decomposition in Pinocchio, it is quite far from the current architecture of Jiminy. Jiminy is providing the whole jacobian as a block matrix, and the connection with individual constraints is already lost at this point.
I suggest adopting the non-aggregated formulation in Jiminy. It would be much easier to take advantage of the sparsity in computations as in Pinocchio, correct PGS bias, and handle friction and non-penetration in solver itself instead of doing confusing index manipulations in PGS itself.
abanow computes the acceleration and forces, no need to do it manually anymore when there is no constraintmodel.armature. No need to overload pinocchio anymore.RigidContraintModelandRigidContraintDataare now provided to handle fixed frame constraints.It is not straightfoward to use the new optimized algorithms for constraint computation implemented in Pinocchio. The complexity comes from the inability to define custom jacobian for the constraints in Pinocchio:
data.J(which is the concatenation in column of the joint jacobian in world frame) by swapping the memory with the constraint Jacobian computed by Jiminy. Then, each constraint would feature an extra attributes corresponding to the associatedRigidContraintModelandRigidContraintData, always assuming 6 fixed DoFs in WORLD frame whatever the actual reference frame. JiminyModelwould provide new methodsgetConstraintModelandgetConstraintDatato get the ordered vectors ofRigidContraintModelandRigidContraintDataattributes for all active constraints respectively. This way, it is not necessary to modify the core algorithms of Pinocchio. However, it would add empty rows in the jacobian, that may break the algorithm.