Minor IK scipy changes

Makes it easier to add custom cost function for scipy ik implements minor speedups of ik

@VinzenzMaennig : You could monkey-patch those changes in your timor version already until this is merged -- I can imagine it being useful for you. Feel free to add further improvements if you want to tackle the ik speed.