added robotpose to kdl frame
minor changes