Merge branch 'develop'
this version can run on almost all models, but the speed is slow. I will add node mapping later
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