Merge branch 'master' into develop
add normal-anti-flip, however hj::SQP will cross the boundary of feasible domain, ipopt is more safe but slow
add degree smooth function
Merge branch 'develop'