Marco Andarcia

@stonier I need access to yocs_msgs :/

@AlexReimann Can you check this pull request? daniel asked me to ask you to merge this one.

@AlexReimann Already fixed the mutex logic. removed the lock from the controlPose() and controlOrientation() functions.