Added forque/torque control for manipulator