Contacts implementation

[hammoudbilal]

Hi @andreadelprete This assumes all contacts are active all the time https://github.com/andreadelprete/consim/blob/327b9ae0b00eec068248a1e1432510dce1b87272/script/robot_simulator_exponential_integrator.py#L295

If I have switching contacts I will have to keep changing the size of the vectors/matrices used, eg: x, intx, int2x ... etc to implement this I will have to resize the Eigen::MatrixXd & Eigen::VectorXd at every integration step, are you aware of more efficient ways for implementing this?

i thought about keeping the maximum size and filling the variables relative to the inactive contacts with zero value, but it seems a bit dangerous to me to do that. Also it makes computing the exponential more expensive as the matrix dimension increases by 6 for every inactive contact.

[andreadelprete]

This assumes all contacts are active all the time

Yes it does.

If I have switching contacts I will have to keep changing the size of the vectors/matrices used, eg: x, intx, int2x ... etc to implement this I will have to resize the Eigen::MatrixXd & Eigen::VectorXd at every integration step, are you aware of more efficient ways for implementing this?

I think keeping the maximum size is a bad idea because the matrix exponential is gonna be the most expensive computation. At first, I think that resizing everything when the number of contacts change is reasonable. Later on, if we feel like improving the code, we could pre-allocate variables for the most common numbers of contacts (e.g., 1, 2, 3 and 4), and use the correct variables depending on the current number of contacts (sacrificing space for speed).