Second commit for Rigid body equation set implementation

Tracker item: 3681