Problem with the Accuracy of the Pose

[kappa95]

Hi community,
I'm working on a Yumi and ROS Kinetic from like 2 months with Moveit for doing a project of picking and placing test tubes. I have setted everything with the ABB drivers but i'm experiencing very difficult issues like: strange reorienting and particularly bad accuracy in the motion. In the sense: if i'm commading to reach a certain position the pose reached is different of like 0.5 cm.
This doesn't happen programming in RAPID.
Have you experiences these kind of issues? Someone solved? I have tried all the possible planners but the only differences are motion not completed. I tried also another IK solver like TrackIK and OMPL planners but nothing solved.
I will share my repository and programs here: YumiProjectThesis

Thank you so much in advance for the help!