wbt_plan_curvature function

Plan curvature