Added implementation of com and com jacobian