Coordinate transformation and on-line planning for position/force control of constrained robots