model update
add large force penalty in COMCost