add com position and jacobian computation