Systems and methods for controlling motion of remotely operated equipment such that a motion path is automatically determined for a plurality of joints of the remotely operated equipment based on an updated target position input received from an operator, a current position of the remotely operated equipment, and predetermined parameters indicative of the geometry of the plurality of joints. An optimized motion path may be provided that avoids detected obstacles and joint singularities of the remotely operated equipment.