Added new cost function that takes into account robot joint movements