added interface for setTargetPoseMatrix
quick fix for unaccessible indices problem
removed unnecessary stuff
fix mixup with function names
removed problematic const