Robotics algorithms