modified to command the vie end effector to a goal position
uses the computevelocity command for velocity control independent of orientation