Open duburcqa opened 3 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.
aba
now computes the acceleration and forces, no need to do it manually anymore when there is no constraintmodel.armature
. No need to overload pinocchio anymore.RigidContraintModel
andRigidContraintData
are 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 associatedRigidContraintModel
andRigidContraintData
, always assuming 6 fixed DoFs in WORLD frame whatever the actual reference frame. JiminyModel
would provide new methodsgetConstraintModel
andgetConstraintData
to get the ordered vectors ofRigidContraintModel
andRigidContraintData
attributes 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.