templates, trying to get it working with ArticulationModelPtr; init done.
initialization corrected
propagation with articulation models works
correction step works with articulation model ptr
correction implemented, printing particles implemented. TODO: check resampling - something is wrong; change printing in particle class using template specialization