CRP6D almost finished. Could be some problems with dynamics. Find bugs