Force inequality cost

Create an inequailty cost for the force. The goals are:

1. positiveness of the force
2. weighted force cost
3. force inside cone.