Added triangle to mark the goal position
Jacobian is now normalized, when the point is reached, dont solve any more

Marker placed at endPosition on initialization
Added several TODO