Add arc-length costs to LeggedRobotPlanningProblem
Add arc-length costs to LeggedRobotPlanningProblem

The arc-length traveled by each hand is penalized

psiorx/drake,psiorx/drake,thientu/drake,psiorx/drake,billhoffman/drake,manuelli/drake,billhoffman/drake,crogers3/drake,billhoffman/drake,crogers3/drake,scottviteri/drake,hanssusilo/drake,sheim/drake,billhoffman/drake,billhoffman/drake,psiorx/drake,manuelli/drake,psiorx/drake,hanssusilo/drake,hanssusilo/drake,thientu/drake,sheim/drake,thientu/drake,crogers3/drake,thientu/drake,sheim/drake,edowson/drake,sheim/drake,scottviteri/drake,edowson/drake,psiorx/drake,scottviteri/drake,manuelli/drake,sheim/drake,billhoffman/drake,hanssusilo/drake,thientu/drake,psiorx/drake,manuelli/drake,crogers3/drake,scottviteri/drake,scottviteri/drake,psiorx/drake,thientu/drake,edowson/drake,scottviteri/drake,hanssusilo/drake,manuelli/drake,manuelli/drake,manuelli/drake,edowson/drake,billhoffman/drake,edowson/drake,edowson/drake,hanssusilo/drake,thientu/drake,hanssusilo/drake,edowson/drake,crogers3/drake,scottviteri/drake,crogers3/drake,sheim/drake,edowson/drake,sheim/drake,crogers3/drake,hanssusilo/drake,crogers3/drake,billhoffman/drake,sheim/drake