Replace hacky p_ scheme in class hierarchy

[dellaert]

Now we have a different type p in Optimizer, Kinematics, etc. replace by an inherited method p() that does a static cast from a single p.